keep in sync with master

follow changes in master
Fixing of errors caused by PX4#19495
autogyro takeoff updats
Fix build of Autogyro take off

Add motor rampup state, killswitch handling
update throttle

Keep upstream

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -733,31 +733,17 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
 	return height_rate_setpoint;
 }
 
+//TADY/////////////////////////////////////////////////////////////////////////////////////////////////TADY
 void
 FixedwingPositionControl::updateManualTakeoffStatus()
 {
-
-	// a VTOL does not need special takeoff handling
-	if (_vehicle_status.is_vtol) {
-		return false;
-	}
-
-	if (_autogyro_takeoff.autogyroTakeoffEnabled()) {
-		return (!_autogyro_takeoff.isInitialized() || _autogyro_takeoff.climbout());
-	}
-
 	if (!_completed_manual_takeoff) {
 		const bool at_controllable_airspeed = _airspeed > _param_fw_airspd_min.get()
 						      || !_airspeed_valid;
 		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 					 && _control_mode.flag_armed;
 		_completed_manual_takeoff = (!_landed && at_controllable_airspeed) || is_hovering;
 	}
-
-	// in air for < 10s
-	return (hrt_elapsed_time(&_time_went_in_air) < 10_s)
-	       && (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get());
-
 }
 
 void
@@ -956,26 +942,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 	}
 
 	/* Copy thrust output for publication, handle special cases */
-	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && // launchdetector only available in auto
-	    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
-	    !_runway_takeoff.runwayTakeoffEnabled() && !_autogyro_takeoff.autogyroTakeoffEnabled()) {
-
-		/* making sure again that the correct thrust is used,
-		 * without depending on library calls for safety reasons.
-		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
-		_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
-
-	} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-		   _runway_takeoff.runwayTakeoffEnabled()) {
-
-		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
-
-	} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-		   _autogyro_takeoff.autogyroTakeoffEnabled()) {
-
-		_att_sp.thrust_body[0] = _autogyro_takeoff.getThrottle(now, min(get_tecs_thrust(), _param_fw_thr_max.get()));
-
-	} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 
 		_att_sp.thrust_body[0] = 0.0f;
 
@@ -1447,23 +1414,39 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_autogyro_takeoff.update(now, _airspeed, _rotor_rpm, _current_altitude - _takeoff_ground_alt,
 					 _current_latitude, _current_longitude, &_mavlink_log_pub);
 
-		float target_airspeed = get_auto_airspeed_setpoint(now,
-					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
-					dt);
-		/*
-		 * Update navigation: _autogyro_takeoff returns the start WP according to mode and phase.
-		 * If we use the navigator heading or not is decided later.
-		 */
+		const float takeoff_airspeed = _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get();
+		float adjusted_min_airspeed = _param_fw_airspd_min.get();
+
+		if (takeoff_airspeed < _param_fw_airspd_min.get()) {
+			// adjust underspeed detection bounds for takeoff airspeed
+			_tecs.set_equivalent_airspeed_min(takeoff_airspeed);
+			adjusted_min_airspeed = takeoff_airspeed;
+		}
+
+		float target_airspeed = get_auto_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed,
+					ground_speed);
+
+
+		const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0),
+						 _runway_takeoff.getStartPosition()(1));
+		const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+
+		// the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded
+		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);
+
 
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			_npfg.navigateWaypoints(_autogyro_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed, _wind_vel);
+			_npfg.navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
+						  _wind_vel, 0.0f);
+
 			_att_sp.roll_body = _autogyro_takeoff.getRoll(_npfg.getRollSetpoint());
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 		} else {
-			_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
+			const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
+			_l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed);
 			_att_sp.roll_body = _autogyro_takeoff.getRoll(_l1_control.get_roll_setpoint());
 		}
 
@@ -1478,7 +1461,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 					   radians(takeoff_pitch_max_deg),
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(), // XXX should we also set autogyro_takeoff_throttle here?
-					   _param_fw_thr_cruise.get(),
 					   _autogyro_takeoff.climbout(),
 					   radians(_autogyro_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())),
 					   tecs_status_s::TECS_MODE_TAKEOFF);
@@ -1488,10 +1470,12 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_att_sp.pitch_body = _autogyro_takeoff.getPitch(get_tecs_pitch());
 
 		// reset integrals except yaw (which also counts for the wheel controller)
-		_att_sp.roll_reset_integral = _autogyro_takeoff.resetIntegrators();
-		_att_sp.pitch_reset_integral = _autogyro_takeoff.resetIntegrators();
+		_att_sp.reset_rate_integrals = _autogyro_takeoff.resetIntegrators();
 
-	} else if (_runway_takeoff.runwayTakeoffEnabled()) {
+	}
+
+/////////////////////////////////////////////////////////////RUNWAY
+	else if (_runway_takeoff.runwayTakeoffEnabled()) {
 		if (!_runway_takeoff.isInitialized()) {
 			_runway_takeoff.init(now, _yaw, global_position);
 
@@ -1608,7 +1592,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
-	} else {
+	}
+
+///////////LOUNCH
+	else {
 		/* Perform launch detection */
 		if (!_skipping_takeoff_detection && _launchDetector.launchDetectionEnabled() &&
 		    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
@@ -1735,6 +1722,33 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt);
 
+	if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()
+	    && !_autogyro_takeoff.autogyroTakeoffEnabled()) {
+
+		/* making sure again that the correct thrust is used,
+		 * without depending on library calls for safety reasons.
+		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
+		_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
+
+	} else if (_runway_takeoff.runwayTakeoffEnabled()) {
+
+		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
+
+	} else if (_autogyro_takeoff.autogyroTakeoffEnabled()) {
+		//PX4_INFO("Spoustim GETTHROTTLE");
+		_att_sp.thrust_body[0] = _autogyro_takeoff.getThrottle(now, get_tecs_thrust());
+
+	} else {
+		/* Copy thrust and pitch values from tecs */
+		// when we are landed state we want the motor to spin at idle speed
+		_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
+	}
+
+	// only use TECS pitch setpoint if launch has not been detected and runway takeoff is not enabled
+	if (!(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())) {
+		_att_sp.pitch_body = get_tecs_pitch();
+	}
+
 	if (!_vehicle_status.in_transition_to_fw) {
 		publishLocalPositionSetpoint(pos_sp_curr);
 	}
@@ -2440,7 +2454,7 @@ FixedwingPositionControl::reset_takeoff_state()
 	_runway_takeoff.reset();
 
 
-    _runway_takeoff.reset();
+	_runway_takeoff.reset();
 	_autogyro_takeoff.reset();
 	_launchDetector.reset();
 	_launch_detection_state = LAUNCHDETECTION_RES_NONE;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -259,6 +259,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	double _current_longitude{0};
 	float _current_altitude{0.f};
 
+	float _roll{0.0f};
 	float _pitch{0.0f};
 	float _yaw{0.0f};
 	float _yawrate{0.0f};
@@ -337,13 +338,11 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	// orbit to altitude only when the aircraft has entered the final *straight approach.
 	hrt_abstime _time_started_landing{0};
 
-<<<<<<< HEAD
 	// [m] lateral touchdown position offset manually commanded during landing
 	float _lateral_touchdown_position_offset{0.0f};
-=======
-	RunwayTakeoff _runway_takeoff;
+
 	AutogyroTakeoff _autogyro_takeoff;
->>>>>>> Autogyro takeoff, squash and clean
+
 
 	// [m] relative vector from land point to approach entrance (NE)
 	Vector2f _landing_approach_entrance_offset_vector{};
@@ -394,12 +393,9 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	float _groundspeed_undershoot{0.0f};			///< ground speed error to min. speed in m/s
 
-	float _roll{0.0f};
-	float _pitch{0.0f};
-	float _yaw{0.0f};
-	float _yawrate{0.0f};
 
-    // TECS
+
+	// TECS
 	// total energy control system - airspeed / altitude control
 	TECS _tecs;