Fix build of Autogyro take off

Author: slimonslimon

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
@@ -1435,25 +1421,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(control_interval,
-					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed);
-		/*
-		 * Update navigation: _autogyro_takeoff returns the start WP according to mode and phase.
-		 * If we use the navigator heading or not is decided later.
-		 */
-		Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
-					 _runway_takeoff.getStartWP()(1));
+		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(prev_wp_local, curr_wp_local, curr_pos_local, 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(prev_wp_local, curr_wp_local, curr_pos_local, 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());
 		}
 
@@ -1468,7 +1468,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);
@@ -1481,7 +1480,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_att_sp.roll_reset_integral = _autogyro_takeoff.resetIntegrators();
 		_att_sp.pitch_reset_integral = _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);
 
@@ -1599,7 +1601,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) {
@@ -1742,7 +1747,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
 
 	} else if (_autogyro_takeoff.autogyroTakeoffEnabled()) {
-		PX4_INFO("Spoustim GETTHROTTLE");
+		//PX4_INFO("Spoustim GETTHROTTLE");
 		_att_sp.thrust_body[0] = _autogyro_takeoff.getThrottle(now, get_tecs_thrust());
 
 	} else {

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,10 +393,7 @@ 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
 	// total energy control system - airspeed / altitude control

src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.cpp

@@ -118,7 +118,8 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 				_time_in_state = now;
 			}
 
-			mavlink_log_info(mavlink_log_pub, "#Takeoff: minimal RPM for prerotator reached");
+			mavlink_log_info(mavlink_log_pub, "Takeoff: minimal RPM for prerotator reached");
+            //PX4_INFO("Takeoff: minimal RPM for prerotator reached");
 		}
 
 		break;
@@ -140,6 +141,7 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 			_time_in_state = now;
 			play_next_tone();
 			mavlink_log_info(mavlink_log_pub, "Takeoff, prerotator RPM reached");
+            //PX4_INFO("Takeoff: prerotator RPM reached");
 		}
 
 		break;
@@ -156,7 +158,7 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 
 			if (rotor_rpm < _param_ag_rotor_flight_rpm.get()) {
 				ready_for_release = false;
-
+                //PX4_INFO("Takeofff, waiting for flight rpm.");
 				// Some histesis needs to be applied for the start interrupt procedure.
 				// Currently, this does not allow the start to be interrupted.
 				//_state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE;
@@ -166,6 +168,7 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 			// check minimal airspeed
 			if (airspeed < (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get())) {
 				ready_for_release = false;
+                //PX4_INFO("Takeofff, waiting for min airspeed.");
 			}
 
 			if (ready_for_release) {
@@ -174,7 +177,7 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 				_state = AutogyroTakeoffState::TAKEOFF_RELEASE;
 				_time_in_state = now;
 				mavlink_log_info(mavlink_log_pub, "Takeoff, Please release.");
-
+                //PX4_INFO("Takeoff, Please release.");
 				play_next_tone();
 			}
 
@@ -198,8 +201,9 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 				play_release_tone();
 			}
 
-			if (alt_agl > _param_rwto_nav_alt.get()) {
+			if (alt_agl > _param_ag_nav_alt.get()) {
 				mavlink_log_info(mavlink_log_pub, "Climbout");
+                PX4_INFO("Takeoff: Climbout.");
 				_state = AutogyroTakeoffState::TAKEOFF_CLIMBOUT;
 				play_next_tone();
 				_time_in_state = now;
@@ -224,6 +228,7 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 			_climbout = false;
 			_state = AutogyroTakeoffState::FLY;
 			_time_in_state = now;
+            PX4_INFO("Takeoff:FLY.");
 		}
 
 		//_climbout = false;
@@ -332,8 +337,8 @@ float AutogyroTakeoff::getRoll(float navigatorRoll)
 	// allow some limited roll during RELEASE and CLIMBOUT
 	else if (_state < AutogyroTakeoffState::FLY) {
 		return math::constrain(navigatorRoll,
-				       math::radians(-_param_rwto_max_roll.get()),
-				       math::radians(_param_rwto_max_roll.get()));
+				       math::radians(-_param_ag_tko_max_roll.get()),
+				       math::radians(_param_ag_tko_max_roll.get()));
 	}
 
 	return navigatorRoll;
@@ -368,7 +373,7 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
 
 	float idle = (double)_param_fw_thr_idle.get();
 
-	PX4_INFO("GET THROTTLE %f, state: %f, time: %f", (double)idle, (double)_state, (double)(now - _time_in_state));
+	//PX4_INFO("GET THROTTLE %f, state: %f, time: %f", (double)idle, (double)_state, (double)(now - _time_in_state));
 
 	switch (_state) {
 
@@ -380,6 +385,7 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
 	case AutogyroTakeoffState::PRE_TAKEOFF_DONE: {
 			if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) {
 				float throttle = ((now - _time_in_state) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
+                //PX4_INFO("Thortle: %f",(double)throttle);
 				return math::min(throttle, _param_rwto_max_thr.get());
 
 			} else {

src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.h

@@ -147,11 +147,9 @@ class __EXPORT AutogyroTakeoff : public ModuleParams
 		(ParamBool<px4::params::AG_TKOFF>) _param_ag_tkoff,
 		(ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
 		(ParamInt<px4::params::RWTO_HDG>) _param_rwto_hdg,
-		(ParamFloat<px4::params::RWTO_NAV_ALT>) _param_rwto_nav_alt,
 		(ParamFloat<px4::params::RWTO_MAX_THR>) _param_rwto_max_thr,
 		(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
 		(ParamFloat<px4::params::RWTO_MAX_PITCH>) _param_rwto_max_pitch,
-		(ParamFloat<px4::params::RWTO_MAX_ROLL>) _param_rwto_max_roll,
 		(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
 		(ParamFloat<px4::params::RWTO_RAMP_TIME>) _param_rwto_ramp_time,
 
@@ -164,7 +162,10 @@ class __EXPORT AutogyroTakeoff : public ModuleParams
 		(ParamFloat<px4::params::AG_PROT_MIN_RPM>) _param_ag_prerotator_minimal_rpm,
 		(ParamFloat<px4::params::AG_PROT_TRG_RPM>) _param_ag_prerotator_target_rpm,
 		(ParamFloat<px4::params::AG_ROTOR_RPM>) _param_ag_rotor_flight_rpm,
-		(ParamInt<px4::params::AG_PROT_TYPE>) _param_ag_prerotator_type
+		(ParamInt<px4::params::AG_PROT_TYPE>) _param_ag_prerotator_type,
+
+		(ParamFloat<px4::params::AG_NAV_ALT>) _param_ag_nav_alt,
+        (ParamFloat<px4::params::AG_TKO_MAX_ROLL>)  _param_ag_tko_max_roll
 
 
 	)

src/modules/fw_pos_control_l1/autogyro_takeoff/autogyro_takeoff_parameters.c

@@ -99,3 +99,33 @@ PARAM_DEFINE_FLOAT(AG_ROTOR_RPM, 1000.0);
  * @value 10 Not implemented: SITL in flightgear
  */
 PARAM_DEFINE_INT32(AG_PROT_TYPE, 0);
+
+
+/**
+ * Altitude AGL at which we have enough ground clearance to allow some roll.
+ *
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @decimal 1
+ * @increment 1
+ * @group Autogyro
+ */
+PARAM_DEFINE_FLOAT(AG_NAV_ALT, 5.0);
+
+/**
+ * Max roll during climbout.
+ *
+ * Roll is limited during climbout to ensure enough lift and prevents aggressive
+ * navigation before we're on a safe height.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 60.0
+ * @decimal 1
+ * @increment 0.5
+ * @group Autogyro
+ */
+
+PARAM_DEFINE_FLOAT(AG_TKO_MAX_ROLL, 25.0);