navigator move parameters out of MissionBlock

- MissionBlock is an interface with > 10 implementations

src/modules/navigator/follow_target.cpp

@@ -400,3 +400,37 @@ bool FollowTarget::target_position_valid()
 	// need at least 1 continuous data points for position estimate
 	return (_target_updates >= 1);
 }
+
+void
+FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
+				     float yaw)
+{
+	if (_navigator->get_land_detected()->landed) {
+		/* landed, don't takeoff, but switch to IDLE mode */
+		item->nav_cmd = NAV_CMD_IDLE;
+
+	} else {
+
+		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
+
+		/* use current target position */
+		item->lat = target.lat;
+		item->lon = target.lon;
+		item->altitude = _navigator->get_home_position()->alt;
+
+		if (min_clearance > 8.0f) {
+			item->altitude += min_clearance;
+
+		} else {
+			item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
+		}
+	}
+
+	item->altitude_is_relative = false;
+	item->yaw = yaw;
+	item->loiter_radius = _navigator->get_loiter_radius();
+	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->time_inside = 0.0f;
+	item->autocontinue = false;
+	item->origin = ORIGIN_ONBOARD;
+}

src/modules/navigator/follow_target.h

@@ -46,6 +46,7 @@
 #include <lib/mathlib/math/Matrix.hpp>
 #include "navigator_mode.h"
 #include "mission_block.h"
+#include <uORB/topics/follow_target.h>
 
 class FollowTarget : public MissionBlock
 {
@@ -162,4 +163,9 @@ private:
 	void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate);
 	void update_target_motion();
 	void update_target_velocity();
+
+	/**
+	 * Set follow_target item
+	 */
+	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
 };

src/modules/navigator/loiter.cpp

@@ -58,7 +58,6 @@
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_param_min_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
 	_loiter_pos_set(false)
 {
@@ -123,7 +122,7 @@ Loiter::set_loiter_position()
 	_loiter_pos_set = true;
 
 	// set current mission item to loiter
-	set_loiter_item(&_mission_item, _param_min_alt.get());
+	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
 
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

src/modules/navigator/loiter.h

@@ -80,7 +80,6 @@ private:
 	 */
 	void set_loiter_position();
 
-	control::BlockParamFloat _param_min_alt;
 	control::BlockParamInt _param_yawmode;
 	bool _loiter_pos_set;
 };

src/modules/navigator/mission.cpp

@@ -66,7 +66,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_dist_between_wps(this, "MIS_DIST_WPS", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
-	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
 	_missionFeasibilityChecker(navigator)
 {
@@ -545,9 +544,7 @@ Mission::set_mission_items()
 	if (item_contains_position(_mission_item)) {
 
 		/* force vtol land */
-		if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
-		    && !_navigator->get_vstatus()->is_rotary_wing
-		    && _navigator->get_vstatus()->is_vtol) {
+		if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
 
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
@@ -732,7 +729,6 @@ Mission::set_mission_items()
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
-			_mission_item.disable_mc_yaw = true;
 		}
 
 		/* we just moved to the landing waypoint, now descend */
@@ -1176,7 +1172,7 @@ Mission::do_abort_landing()
 	//  or 2 * FW_CLMBOUT_DIFF above the current altitude
 	float alt_landing = get_absolute_altitude_for_item(_mission_item);
 	float min_climbout = _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get());
-	float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), min_climbout);
+	float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), min_climbout);
 
 	// turn current landing waypoint into an indefinite loiter
 	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;

src/modules/navigator/mission.h

@@ -242,7 +242,6 @@ private:
 	control::BlockParamFloat _param_dist_between_wps;
 	control::BlockParamInt _param_altmode;
 	control::BlockParamInt _param_yawmode;
-	control::BlockParamInt _param_force_vtol;
 	control::BlockParamFloat _param_fw_climbout_diff;
 
 	struct mission_s _onboard_mission {};

src/modules/navigator/mission_block.cpp

@@ -56,13 +56,8 @@
 
 MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
-	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_yaw_timeout(this, "MIS_YAW_TMT", false),
 	_param_yaw_err(this, "MIS_YAW_ERR", false),
-	_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
-	_param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false),
-	_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false),
-	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false),
 	_param_reverse_delay(this, "VT_B_REV_DEL", false)
 {
@@ -422,19 +417,21 @@ MissionBlock::issue_command(const mission_item_s &item)
 	}
 
 	if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
-		PX4_INFO("do_set_servo command");
+		PX4_INFO("DO_SET_SERVO command");
+
 		// XXX: we should issue a vehicle command and handle this somewhere else
-		_actuators = {};
+		actuator_controls_s actuators = {};
+		actuators.timestamp = hrt_absolute_time();
+
 		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
 		// params[1] new value for selected actuator in ms 900...2000
-		_actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
-		_actuators.timestamp = hrt_absolute_time();
+		actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
 
 		if (_actuator_pub != nullptr) {
-			orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
+			orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
 
 		} else {
-			_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
+			_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
 		}
 
 	} else {
@@ -498,7 +495,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 			    _navigator->get_loiter_radius();
 	sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
 	sp->acceptance_radius = item.acceptance_radius;
-	sp->disable_mc_yaw_control = item.disable_mc_yaw;
 
 	sp->cruising_speed = _navigator->get_cruising_speed();
 	sp->cruising_throttle = _navigator->get_cruising_throttle();
@@ -527,29 +523,19 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 
 	case NAV_CMD_VTOL_TAKEOFF:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
-
-		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()) {
-			sp->disable_mc_yaw_control = true;
-		}
-
 		break;
 
 	case NAV_CMD_LAND:
 	case NAV_CMD_VTOL_LAND:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
-
-		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) {
-			sp->disable_mc_yaw_control = true;
-		}
-
 		break;
 
 	case NAV_CMD_LOITER_TO_ALT:
 
 		// initially use current altitude, and switch to mission item altitude once in loiter position
-		if (_param_loiter_min_alt.get() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1)
+		if (_navigator->get_loiter_min_alt() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1)
 			sp->alt = math::max(_navigator->get_global_position()->alt,
-					    _navigator->get_home_position()->alt + _param_loiter_min_alt.get());
+					    _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt());
 
 		} else {
 			sp->alt = _navigator->get_global_position()->alt;
@@ -559,11 +545,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	case NAV_CMD_LOITER_TIME_LIMIT:
 	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
-
-		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
-			sp->disable_mc_yaw_control = true;
-		}
-
 		break;
 
 	default:
@@ -625,40 +606,6 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 	}
 }
 
-void
-MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
-				     float yaw)
-{
-	if (_navigator->get_land_detected()->landed) {
-		/* landed, don't takeoff, but switch to IDLE mode */
-		item->nav_cmd = NAV_CMD_IDLE;
-
-	} else {
-
-		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
-
-		/* use current target position */
-		item->lat = target.lat;
-		item->lon = target.lon;
-		item->altitude = _navigator->get_home_position()->alt;
-
-		if (min_clearance > 8.0f) {
-			item->altitude += min_clearance;
-
-		} else {
-			item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
-		}
-	}
-
-	item->altitude_is_relative = false;
-	item->yaw = yaw;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
-	item->time_inside = 0.0f;
-	item->autocontinue = false;
-	item->origin = ORIGIN_ONBOARD;
-}
-
 void
 MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch)
 {
@@ -681,16 +628,13 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude,
 void
 MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location)
 {
-
 	/* VTOL transition to RW before landing */
-	if (_navigator->get_vstatus()->is_vtol &&
-	    !_navigator->get_vstatus()->is_rotary_wing &&
-	    _param_force_vtol.get() == 1) {
+	if (_navigator->force_vtol()) {
 
-		vehicle_command_s cmd = {};
-		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
-		cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-		_navigator->publish_vehicle_cmd(&cmd);
+		vehicle_command_s vcmd = {};
+		vcmd.command = NAV_CMD_DO_VTOL_TRANSITION;
+		vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+		_navigator->publish_vehicle_cmd(&vcmd);
 	}
 
 	/* set the land item */

src/modules/navigator/mission_block.h

@@ -44,8 +44,6 @@
 #include "navigator_mode.h"
 
 #include <navigator/navigation.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/follow_target.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_command.h>
@@ -115,11 +113,6 @@ protected:
 	 */
 	void set_idle_item(struct mission_item_s *item);
 
-	/**
-	 * Set follow_target item
-	 */
-	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
-
 	/**
 	 * General function used to adjust the mission item based on vehicle specific limitations
 	 */
@@ -138,16 +131,12 @@ protected:
 	hrt_abstime _action_start{0};
 	hrt_abstime _time_wp_reached{0};
 
-	actuator_controls_s _actuators{};
 	orb_advert_t    _actuator_pub{nullptr};
 
-	control::BlockParamFloat _param_loiter_min_alt;
 	control::BlockParamFloat _param_yaw_timeout;
 	control::BlockParamFloat _param_yaw_err;
-	control::BlockParamInt _param_vtol_wv_land;
-	control::BlockParamInt _param_vtol_wv_takeoff;
-	control::BlockParamInt _param_vtol_wv_loiter;
-	control::BlockParamInt _param_force_vtol;
+
+	// VTOL parameters
 	control::BlockParamFloat _param_back_trans_dec_mss;
 	control::BlockParamFloat _param_reverse_delay;
 };

src/modules/navigator/mission_params.c

@@ -174,41 +174,3 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
-
-/**
- * Weather-vane mode landings for missions
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
-
-/**
- * Enable weather-vane mode takeoff for missions
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0);
-
-/**
- * Weather-vane mode for loiter
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
-
-/**
- * Cruise Airspeed
- *
- * The fixed wing controller tries to fly at this airspeed.
- *
- * @unit m/s
- * @min 0.0
- * @max 40
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);

src/modules/navigator/navigation.h

@@ -141,7 +141,6 @@ struct mission_item_s {
 			 force_heading : 1,				/**< heading needs to be reached */
 			 altitude_is_relative : 1,		/**< true if altitude is relative from start point	*/
 			 autocontinue : 1,				/**< true if next waypoint should follow after this one */
-			 disable_mc_yaw : 1,				/**< weathervane mode */
 			 vtol_back_transition : 1;		/**< part of the vtol back transition sequence */
 	};
 };

src/modules/navigator/navigator.h

@@ -222,6 +222,10 @@ public:
 
 	bool		abort_landing();
 
+	float		get_loiter_min_alt() const { return _param_loiter_min_alt.get(); }
+
+	bool		force_vtol() const { return _vstatus.is_vtol && !_vstatus.is_rotary_wing && _param_force_vtol.get(); }
+
 private:
 
 	bool		_task_should_exit{false};	/**< if true, sensor task should exit */
@@ -292,11 +296,15 @@ private:
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
+	// navigator parameters
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
-
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
 	control::BlockParamFloat _param_fw_alt_acceptance_radius;	/**< acceptance radius for fixedwing altitude */
 	control::BlockParamFloat _param_mc_alt_acceptance_radius;	/**< acceptance radius for multicopter altitude */
+	control::BlockParamInt _param_force_vtol;	/**< acceptance radius for multicopter altitude */
+
+	// non-navigator parameters
+	control::BlockParamFloat _param_loiter_min_alt;
 
 	float _mission_cruising_speed_mc{-1.0f};
 	float _mission_cruising_speed_fw{-1.0f};

src/modules/navigator/navigator_main.cpp

@@ -46,6 +46,9 @@
 #include "navigator.h"
 
 #include <cfloat>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
 
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
@@ -55,9 +58,6 @@
 #include <px4_defines.h>
 #include <px4_posix.h>
 #include <px4_tasks.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-#include <sys/types.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
@@ -96,10 +96,14 @@ Navigator::Navigator() :
 	_engineFailure(this, "EF"),
 	_gpsFailure(this, "GPSF"),
 	_follow_target(this, "TAR"),
+	// navigator params
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),
-	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD")
+	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
+	_param_force_vtol(this, "FORCE_VT"),
+	// non-navigator params
+	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;

src/modules/navigator/rtl.cpp

@@ -54,7 +54,6 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RTL_RETURN_ALT", false),
-	_param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
 	_param_land_delay(this, "RTL_LAND_DELAY", false),
 	_param_rtl_min_dist(this, "RTL_MIN_DIST", false)

src/modules/navigator/rtl.h

@@ -92,7 +92,6 @@ private:
 	} _rtl_state{RTL_STATE_NONE};
 
 	control::BlockParamFloat _param_return_alt;
-	control::BlockParamFloat _param_min_loiter_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
 	control::BlockParamFloat _param_rtl_min_dist;