Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -199,8 +199,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	}
 
 	/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
-	BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits
+	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
-	BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits
+	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
 	if (mode == TECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
@@ -218,12 +218,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
 	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
 	 * is running) */
-	bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
+	bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
-			_controlTotalEnergy.getOutputLimiter() :
-			*outputLimiterThrottle,
-			outputLimiterPitch == NULL ?
-			_controlEnergyDistribution.getOutputLimiter() :
-			*outputLimiterPitch);
 
 	/* Write part of the status message */
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;

src/modules/navigator/loiter.cpp

@@ -53,8 +53,7 @@
 #include "loiter.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
+	MissionBlock(navigator, name)
-	MissionBlock(navigator)
 {
 	/* load initial params */
 	updateParams();

src/modules/navigator/loiter.h

@@ -47,7 +47,7 @@
 #include "navigator_mode.h"
 #include "mission_block.h"
 
-class Loiter : public NavigatorMode, MissionBlock
+class Loiter : public MissionBlock
 {
 public:
 	/**

src/modules/navigator/mission.cpp

@@ -58,8 +58,7 @@
 #include "mission.h"
 
 Mission::Mission(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
+	MissionBlock(navigator, name),
-	MissionBlock(navigator),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),

src/modules/navigator/mission.h

@@ -62,7 +62,7 @@
 
 class Navigator;
 
-class Mission : public NavigatorMode, MissionBlock
+class Mission : public MissionBlock
 {
 public:
 	/**

src/modules/navigator/mission_block.cpp

@@ -52,13 +52,13 @@
 #include "mission_block.h"
 
 
-MissionBlock::MissionBlock(Navigator *navigator) :
+MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_mission_item({0}),
-	_mission_item_valid(false),
+	_mission_item_valid(false)
-	_navigator_priv(navigator)
 {
 }
 
@@ -70,7 +70,7 @@ bool
 MissionBlock::is_mission_item_reached()
 {
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _navigator_priv->get_vstatus()->condition_landed;
+		return _navigator->get_vstatus()->condition_landed;
 	}
 
 	/* TODO: count turns */
@@ -88,24 +88,24 @@ MissionBlock::is_mission_item_reached()
 		float dist_z = -1.0f;
 
 		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator_priv->get_home_position()->alt
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
 			              : _mission_item.altitude;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator_priv->get_global_position()->lat,
+				                          _navigator->get_global_position()->lat,
-							  _navigator_priv->get_global_position()->lon,
+							  _navigator->get_global_position()->lon,
-							  _navigator_priv->get_global_position()->alt,
+							  _navigator->get_global_position()->alt,
 				&dist_xy, &dist_z);
 
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter */
-			if (_navigator_priv->get_global_position()->alt >
+			if (_navigator->get_global_position()->alt >
-				altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
+				altitude_amsl - _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
@@ -119,10 +119,10 @@ MissionBlock::is_mission_item_reached()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
-		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
 
 			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
 
 			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
 				_waypoint_yaw_reached = true;
@@ -167,7 +167,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->valid = true;
 	sp->lat = item->lat;
 	sp->lon = item->lon;
-	sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
 	sp->yaw = item->yaw;
 	sp->loiter_radius = item->loiter_radius;
 	sp->loiter_direction = item->loiter_direction;
@@ -211,25 +211,25 @@ bool
 MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
     /* don't change setpoint if 'can_loiter_at_sp' flag set */
-	if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
+	if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
 		/* use current position */
-		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
+		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
 		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
 
-	    _navigator_priv->set_can_loiter_at_sp(true);
+	    _navigator->set_can_loiter_at_sp(true);
 	}
 
     if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
-            || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
             || pos_sp_triplet->current.loiter_direction != 1
             || pos_sp_triplet->previous.valid
             || !pos_sp_triplet->current.valid
             || pos_sp_triplet->next.valid) {
         /* position setpoint triplet should be updated */
         pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-        pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+        pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
         pos_sp_triplet->current.loiter_direction = 1;
 
         pos_sp_triplet->previous.valid = false;

src/modules/navigator/mission_block.h

@@ -47,17 +47,17 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 
+#include "navigator_mode.h"
+
 class Navigator;
 
-class MissionBlock
+class MissionBlock : public NavigatorMode
 {
 public:
 	/**
 	 * Constructor
-	 *
-	 * @param pointer to parent class
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, const char *name);
 
 	/**
 	 * Destructor
@@ -101,9 +101,6 @@ public:
 
 	mission_item_s _mission_item;
 	bool _mission_item_valid;
-
-private:
-	Navigator *_navigator_priv;
 };
 
 #endif

src/modules/navigator/navigator.h

@@ -115,7 +115,7 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
-	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
+	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
@@ -165,7 +165,7 @@ private:
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
-	control::BlockParamFloat _param_takeoff_acceptance_radius;	/**< acceptance for takeoff */
+	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
 	/**
 	 * Retrieve global position
 	 */

src/modules/navigator/navigator_main.cpp

@@ -123,7 +123,7 @@ Navigator::Navigator() :
 	_rtl(this, "RTL"),
 	_update_triplet(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
-	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
+	_param_acceptance_radius(this, "ACC_RAD")
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;

src/modules/navigator/navigator_params.c

@@ -55,12 +55,12 @@
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
 /**
- * Takeoff Acceptance Radius (FW only)
+ * Acceptance Radius
  *
- * Acceptance radius for fixedwing.
+ * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
  * @min 1.0
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);

src/modules/navigator/rtl.cpp

@@ -56,13 +56,11 @@
 #define DELAY_SIGMA	0.01f
 
 RTL::RTL(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
+	MissionBlock(navigator, name),
-	MissionBlock(navigator),
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY"),
+	_param_land_delay(this, "LAND_DELAY")
-	_param_acceptance_radius(this, "ACCEPT_RAD")
 {
 	/* load initial params */
 	updateParams();
@@ -147,7 +145,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -178,7 +176,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -198,7 +196,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = false;
@@ -220,7 +218,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = autoland;
@@ -246,7 +244,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -265,7 +263,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_IDLE;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;

src/modules/navigator/rtl.h

@@ -54,7 +54,7 @@
 
 class Navigator;
 
-class RTL : public NavigatorMode, MissionBlock
+class RTL : public MissionBlock
 {
 public:
 	/**
@@ -105,7 +105,6 @@ private:
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
-	control::BlockParamFloat _param_acceptance_radius;
 };
 
 #endif

src/modules/navigator/rtl_params.c

@@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
-
-/**
- * RTL acceptance radius
- *
- * Acceptance radius for waypoints set for RTL
- *
- * @unit meters
- * @min 1
- * @max
- * @group RTL
- */
-PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);