split takeoff into 2 phases, reseting integrators when still on runway

msg/vehicle_attitude_setpoint.msg

@@ -22,4 +22,4 @@ bool roll_reset_integral			# Reset roll integral part (navigation logic change)
 bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
 bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
 
-bool fw_control_wheel					# control heading with rudder (used for auto takeoff on runway)
+bool fw_control_yaw					# control heading with rudder (used for auto takeoff on runway)

src/lib/runway_takeoff/RunwayTakeoff.cpp

@@ -59,6 +59,7 @@ RunwayTakeoff::RunwayTakeoff() :
 	_init_yaw(0),
 	_climbout(false),
 	_min_airspeed_scaling(1.3f),
+	_max_takeoff_roll(15.0f),
 	_runway_takeoff_enabled(this, "TKOFF"),
 	_runway_takeoff_heading(this, "HDG"),
 	_runway_takeoff_nav_alt(this, "NAV_ALT"),
@@ -111,7 +112,15 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
 		break;
 
 	case RunwayTakeoffState::TAKEOFF:
-		if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) {
+		if (alt_agl > _runway_takeoff_nav_alt.get()) {
+			_state = RunwayTakeoffState::CLIMBOUT;
+			mavlink_log_info(mavlink_fd, "#Climbout");
+		}
+
+		break;
+
+	case RunwayTakeoffState::CLIMBOUT:
+		if (alt_agl > _climbout_diff.get()) {
 			_state = RunwayTakeoffState::FLY;
 			mavlink_log_info(mavlink_fd, "#Navigating to waypoint");
 		}
@@ -123,10 +132,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
 	}
 }
 
-bool RunwayTakeoff::controlWheel()
+bool RunwayTakeoff::controlYaw()
 {
-	// keep controlling wheel until takeoff
+	// keep controlling yaw directly until we start navigation
-	return _state < RunwayTakeoffState::TAKEOFF;
+	return _state < RunwayTakeoffState::CLIMBOUT;
 }
 
 float RunwayTakeoff::getPitch(float tecsPitch)
@@ -141,10 +150,17 @@ float RunwayTakeoff::getPitch(float tecsPitch)
 float RunwayTakeoff::getRoll(float navigatorRoll)
 {
 	// until we have enough ground clearance, set roll to 0
-	if (_state < RunwayTakeoffState::FLY) {
+	if (_state < RunwayTakeoffState::CLIMBOUT) {
 		return 0.0f;
 	}
 
+	// allow some roll during climbout
+	else if (_state < RunwayTakeoffState::FLY) {
+		return math::constrain(navigatorRoll,
+				math::radians(-_max_takeoff_roll),
+				math::radians(_max_takeoff_roll));
+	}
+
 	return navigatorRoll;
 }
 
@@ -186,7 +202,8 @@ float RunwayTakeoff::getThrottle(float tecsThrottle)
 
 bool RunwayTakeoff::resetIntegrators()
 {
-	return _state <= RunwayTakeoffState::TAKEOFF;
+	// reset integrators if we're still on runway
+	return _state < RunwayTakeoffState::TAKEOFF;
 }
 
 float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)

src/lib/runway_takeoff/RunwayTakeoff.h

@@ -55,10 +55,11 @@ namespace runwaytakeoff
 {
 
 enum RunwayTakeoffState {
-	THROTTLE_RAMP = 0, /**<  */
+	THROTTLE_RAMP = 0, /**< ramping up throttle */
-	CLAMPED_TO_RUNWAY = 1, /**<  */
+	CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */
-	TAKEOFF = 2, /**<  */
+	TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */
-	FLY = 3 /**<  */
+	CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */
+	FLY = 4 /**< fly towards takeoff waypoint */
 };
 
 class __EXPORT RunwayTakeoff : public control::SuperBlock
@@ -75,7 +76,7 @@ public:
 
 	bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); };
 
-	bool controlWheel();
+	bool controlYaw();
 	bool climbout() { return _climbout; };
 	float getPitch(float tecsPitch);
 	float getRoll(float navigatorRoll);
@@ -98,6 +99,7 @@ private:
 
 	/** magic values **/
 	float _min_airspeed_scaling;
+	float _max_takeoff_roll;
 
 	/** parameters **/
 	control::BlockParamInt _runway_takeoff_enabled;

src/lib/runway_takeoff/runway_takeoff_params.c

@@ -66,10 +66,10 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
 PARAM_DEFINE_INT32(RWTO_HDG, 0);
 
 /**
- * Altitude AGL at which navigation towards takeoff waypoint starts.
+ * Altitude AGL at which we have enough ground clearance to allow some roll.
  * Until RWTO_NAV_ALT is reached the plane is held level and only
- * rudder is used to keep the heading (see RWTO_HDG). If this is lower
+ * rudder is used to keep the heading (see RWTO_HDG). This should be below
- * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead.
+ * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0.
  *
  * @min 0.0
  * @max 100.0

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -1046,7 +1046,7 @@ FixedwingAttitudeControl::task_main()
 					}
 
 					float yaw_u = 0.0f;
-					if (_att_sp.fw_control_wheel == true) {
+					if (_att_sp.fw_control_yaw == true) {
 						yaw_u = _wheel_ctrl.control_bodyrate(control_input);
 					}
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	bool setpoint = true;
 
-	_att_sp.fw_control_wheel = false;		// by default we don't want yaw to be contoller directly with rudder
+	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -1371,6 +1371,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				}
 
 				// 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);
 				float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
 
@@ -1406,8 +1407,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				// assign values
 				_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll());
 				_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
-				_att_sp.fw_control_wheel = _runway_takeoff.controlWheel();
+				_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 				_att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand());
+
+				_att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators();
+				_att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators();
+				_att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators();
+
 				/*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body,
 					(double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/