don't use virtual line anymore for takeoff but use correct starting point to navigate, updated default parameters for wheel controller

src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp

@@ -111,8 +111,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
     _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
             _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
             integrator_constrained;
-    /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f",
-            (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/
+    /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
+            (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/
 
 
     return math::constrain(_last_output, -1.0f, 1.0f);

src/lib/runway_takeoff/RunwayTakeoff.cpp

@@ -58,9 +58,8 @@ RunwayTakeoff::RunwayTakeoff() :
 	_initialized_time(0),
 	_init_yaw(0),
 	_climbout(false),
-	_start_sp{},
-	_target_sp{},
 	_throttle_ramp_time(2 * 1e6),
+	_start_wp(),
 	_runway_takeoff_enabled(this, "TKOFF"),
 	_heading_mode(this, "HDG"),
 	_nav_alt(this, "NAV_ALT"),
@@ -81,16 +80,19 @@ RunwayTakeoff::~RunwayTakeoff()
 
 }
 
-void RunwayTakeoff::init(float yaw)
+void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
 {
 	_init_yaw = yaw;
 	_initialized = true;
 	_state = RunwayTakeoffState::THROTTLE_RAMP;
 	_initialized_time = hrt_absolute_time();
 	_climbout = true; // this is true until climbout is finished
+	_start_wp(0) = (float)current_lat;
+	_start_wp(1) = (float)current_lon;
 }
 
-void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
+void RunwayTakeoff::update(float airspeed, float alt_agl,
+	double current_lat, double current_lon, int mavlink_fd)
 {
 
 	switch (_state) {
@@ -112,6 +114,16 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
 	case RunwayTakeoffState::TAKEOFF:
 		if (alt_agl > _nav_alt.get()) {
 			_state = RunwayTakeoffState::CLIMBOUT;
+
+			/*
+			 * If we started in heading hold mode, move the navigation start WP to the current location now.
+			 * The navigator will take this as starting point to navigate towards the takeoff WP.
+			 */
+			if (_heading_mode.get() == 0) {
+				_start_wp(0) = (float)current_lat;
+				_start_wp(1) = (float)current_lon;
+			}
+
 			mavlink_log_info(mavlink_fd, "#Climbout");
 		}
 
@@ -127,7 +139,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
 		break;
 
 	default:
-		return;
+		break;
 	}
 }
 
@@ -177,10 +189,18 @@ float RunwayTakeoff::getRoll(float navigatorRoll)
 
 /*
  * Returns the yaw setpoint to use.
+ *
+ * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the
+ * runway. When it has enough ground clearance we start navigation towards WP.
  */
 float RunwayTakeoff::getYaw(float navigatorYaw)
 {
-	return navigatorYaw;
+	if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) {
+		return _init_yaw;
+
+	} else {
+		return navigatorYaw;
+	}
 }
 
 /*
@@ -252,31 +272,9 @@ float RunwayTakeoff::getMaxPitch(float max)
 /*
  * Returns the "previous" (start) WP for navigation.
  */
-math::Vector<2> RunwayTakeoff::getPrevWP()
+math::Vector<2> RunwayTakeoff::getStartWP()
 {
-	math::Vector<2> prev_wp;
-	prev_wp(0) = (float)_start_sp.lat;
-	prev_wp(1) = (float)_start_sp.lon;
-	return prev_wp;
-}
-
-/*
- * Returns the "current" (target) WP for navigation.
- */
-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;
-	}
-	
+	return _start_wp;
 }
 
 void RunwayTakeoff::reset()

src/lib/runway_takeoff/RunwayTakeoff.h

@@ -68,16 +68,14 @@ public:
 	RunwayTakeoff();
 	~RunwayTakeoff();
 
-	void init(float yaw);
-	void update(float airspeed, float alt_agl, int mavlink_fd);
+	void init(float yaw, double current_lat, double current_lon);
+	void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd);
 
 	RunwayTakeoffState getState() { return _state; };
 	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();
@@ -89,8 +87,7 @@ public:
 	bool resetIntegrators();
 	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);
+	math::Vector<2> getStartWP();
 
 	void reset();
 
@@ -102,9 +99,8 @@ private:
 	hrt_abstime _initialized_time;
 	float _init_yaw;
 	bool _climbout;
-	struct position_setpoint_s _start_sp;
-	struct position_setpoint_s _target_sp;
 	unsigned _throttle_ramp_time;
+	math::Vector<2> _start_wp;
 
 	/** parameters **/
 	control::BlockParamInt _runway_takeoff_enabled;

src/modules/fw_att_control/fw_att_control_params.c

@@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
  * @max 1.0
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f);
 
 /**
  * Wheel steering rate integrator gain
@@ -254,7 +254,7 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f);
  * @max 1.0
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f);
+PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f);
 
 /**
  * Maximum wheel steering rate

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1375,45 +1375,31 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			if (_runway_takeoff.runwayTakeoffEnabled()) {
 				if (!_runway_takeoff.isInitialized()) {
-					_runway_takeoff.init(_att.yaw);
+					_runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon);
 
 					/* 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 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
 				_runway_takeoff.update(
 					_airspeed.true_airspeed_m_s,
 					_global_pos.alt - terrain_alt,
+					_global_pos.lat,
+					_global_pos.lon,
 					_mavlink_fd);
 
+				/*
+				 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
+				 * If we use the navigator heading or not is decided later.
+				 */
+				_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d);
+
 				// update tecs
 				float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max);
 				float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);