use navigator to hold heading

src/lib/runway_takeoff/RunwayTakeoff.cpp

@@ -47,6 +47,7 @@
 #include <controllib/block/BlockParam.hpp>
 #include <mavlink/mavlink_log.h>
 #include <mathlib/mathlib.h>
+#include <math.h>
 
 namespace runwaytakeoff
 {
@@ -58,6 +59,8 @@ RunwayTakeoff::RunwayTakeoff() :
 	_initialized_time(0),
 	_init_yaw(0),
 	_climbout(false),
+	_start_sp{},
+	_target_sp{},
 	_runway_takeoff_enabled(this, "TKOFF"),
 	_heading_mode(this, "HDG"),
 	_nav_alt(this, "NAV_ALT"),
@@ -150,17 +153,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll)
 		return 0.0f;
 	}
 
-	// allow some roll during climbout if waypoint heading is targeted
+	// allow some roll during climbout
 	else if (_state < RunwayTakeoffState::FLY) {
-		if (_heading_mode.get() == 0) {
-			// otherwise stay at 0 roll
-			return 0.0f;
-
-		} else if (_heading_mode.get() == 1) {
-			return math::constrain(navigatorRoll,
-					       math::radians(-_max_takeoff_roll.get()),
-					       math::radians(_max_takeoff_roll).get());
-		}
+		return math::constrain(navigatorRoll,
+				       math::radians(-_max_takeoff_roll.get()),
+				       math::radians(_max_takeoff_roll.get()));
 	}
 
 	return navigatorRoll;
@@ -168,18 +165,6 @@ float RunwayTakeoff::getRoll(float navigatorRoll)
 
 float RunwayTakeoff::getYaw(float navigatorYaw)
 {
-	if (_state < RunwayTakeoffState::FLY) {
-		if (_heading_mode.get() == 0) {
-			// fix heading in the direction the airframe points
-			return _init_yaw;
-
-		} else if (_heading_mode.get() == 1) {
-			// or head into the direction of the takeoff waypoint
-			// XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground)
-			return navigatorYaw;
-		}
-	}
-
 	return navigatorYaw;
 }
 
@@ -230,6 +215,30 @@ float RunwayTakeoff::getMaxPitch(float max)
 	}
 }
 
+math::Vector<2> RunwayTakeoff::getPrevWP()
+{
+	math::Vector<2> prev_wp;
+	prev_wp(0) = (float)_start_sp.lat;
+	prev_wp(1) = (float)_start_sp.lon;
+	return prev_wp;
+}
+
+math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp)
+{
+	if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) {
+		// navigating towards calculated direction if heading mode 0 and as long as we're in climbout
+		math::Vector<2> curr_wp;
+		curr_wp(0) = (float)_target_sp.lat;
+		curr_wp(1) = (float)_target_sp.lon;
+		return curr_wp;
+
+	} else {
+		// navigating towards next mission waypoint
+		return sp_curr_wp;
+	}
+	
+}
+
 void RunwayTakeoff::reset()
 {
 	_initialized = false;

src/lib/runway_takeoff/RunwayTakeoff.h

@@ -50,6 +50,7 @@
 #include <controllib/block/BlockParam.hpp>
 #include <mavlink/mavlink_log.h>
 #include <mathlib/mathlib.h>
+#include <math.h>
 
 namespace runwaytakeoff
 {
@@ -75,6 +76,10 @@ public:
 	bool isInitialized() { return _initialized; };
 
 	bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); };
+	float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); };
+	position_setpoint_s *getStartSP() { return &_start_sp; };
+	position_setpoint_s *getTargetSP() { return &_target_sp; };
+	float getInitYaw() { return _init_yaw; };
 
 	bool controlYaw();
 	bool climbout() { return _climbout; };
@@ -83,9 +88,10 @@ public:
 	float getYaw(float navigatorYaw);
 	float getThrottle(float tecsThrottle);
 	bool resetIntegrators();
-	float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); };
 	float getMinPitch(float sp_min, float climbout_min, float min);
 	float getMaxPitch(float max);
+	math::Vector<2> getPrevWP();
+	math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp);
 
 	void reset();
 
@@ -97,6 +103,8 @@ private:
 	hrt_abstime _initialized_time;
 	float _init_yaw;
 	bool _climbout;
+	struct position_setpoint_s _start_sp;
+	struct position_setpoint_s _target_sp;
 
 	/** parameters **/
 	control::BlockParamInt _runway_takeoff_enabled;

src/lib/runway_takeoff/runway_takeoff_params.c

@@ -110,17 +110,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
  */
 PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
 
-/**
- * Max pitch during takeoff.
- * Fixed-wing settings are used if set to 0. Note that there is also a minimum
- * pitch of 10 degrees during takeoff, so this must be larger if set.
- *
- * @min 0.0
- * @max 60.0
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
-
 /**
  * Max roll during climbout.
  * Roll is limited during climbout to ensure enough lift and prevents aggressive

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1368,12 +1368,32 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					/* need this already before takeoff is detected
 					 * doesn't matter if it gets reset when takeoff is detected eventually */
 					_takeoff_ground_alt = _global_pos.alt;
+
+					// draw a line from takeoff location into the direction the UAV is heading
+					get_waypoint_heading_distance(
+							_runway_takeoff.getInitYaw(),
+							HDG_HOLD_DIST_NEXT,
+							*_runway_takeoff.getStartSP(),
+							*_runway_takeoff.getTargetSP(),
+							true);
+
 					mavlink_log_info(_mavlink_fd, "#Takeoff on runway");
 				}
 
-				// update navigation
-				// FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+				// update takeoff path if we're reaching the end of it
+				if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
+					_runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) {
+					get_waypoint_heading_distance(
+							_runway_takeoff.getInitYaw(),
+							HDG_HOLD_DIST_NEXT,
+							*_runway_takeoff.getStartSP(),
+							*_runway_takeoff.getTargetSP(),
+							false);
+				}
+
+				/* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet
+				 * or the calculated one through initial heading */
+				_l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d);
 				float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
 
 				// update runway takeoff helper