Merged release into master

2026-10-24 10:13:34 +08:00 · 2015-06-13 11:06:01 +02:00
parent 2fc069bd63 a0176474c7
commit c9fefe236b
47 changed files with 1036 additions and 344 deletions
@@ -94,6 +94,13 @@
 #include <platforms/px4_defines.h>

 static int	_control_task = -1;			/**< task handle for sensor task */
+#define HDG_HOLD_DIST_NEXT 			3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
+#define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
+#define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
+#define HDG_HOLD_YAWRATE_THRESH 	0.1f 		// max yawrate at which plane locks yaw for heading hold mode
+#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 	// max manual roll input from user which does not change the locked heading
+
+#define THROTTLE_THRESH 0.05f 	// max throttle from user which will not lead to motors spinning up in altitude controlled modes


 /**
@@ -165,7 +172,13 @@ private:

 	perf_counter_t	_loop_perf;			/**< loop performance counter */

-	float	_hold_alt;				/**< hold altitude for velocity mode */
+	float	_hold_alt;				/**< hold altitude for altitude mode */
+	float 	_ground_alt;			/**< ground altitude at which plane was launched */
+	float	_hdg_hold_yaw;				/**< hold heading for velocity mode */
+	bool	_hdg_hold_enabled;			/**< heading hold enabled */
+	bool 	_yaw_lock_engaged;			/**< yaw is locked for heading hold */
+	struct position_setpoint_s _hdg_hold_prev_wp;	/**< position where heading hold started */
+	struct position_setpoint_s _hdg_hold_curr_wp;	/**< position to which heading hold flies */
 	hrt_abstime _control_position_last_called; /**<last call of control_position  */

 	/* land states */
@@ -176,6 +189,9 @@ private:
 	bool land_onslope;
 	bool land_useterrain;

+	bool _was_in_air;	/**< indicated wether the plane was in the air in the previous interation*/
+	hrt_abstime _time_went_in_air;	/**< time at which the plane went in the air */
+
 	/* takeoff/launch states */
 	LaunchDetectionResult launch_detection_state;

@@ -205,6 +221,7 @@ private:
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
 		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current;			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.

@@ -355,6 +372,17 @@ private:
 	 */
 	void navigation_capabilities_publish();

+	/**
+	 * Get a new waypoint based on heading and distance from current position
+	 *
+	 * @param heading the heading to fly to
+	 * @param distance the distance of the generated waypoint
+	 * @param waypoint_prev the waypoint at the current position
+	 * @param waypoint_next the waypoint in the heading direction
+	 */
+	void		get_waypoint_heading_distance(float heading, float distance,
+					struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init);
+
 	/**
 	 * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
 	 */
@@ -363,6 +391,17 @@ private:
 	/**
 	 * Control position.
 	 */
+
+	/**
+	 *	Do takeoff help when in altitude controlled modes
+	 */
+	 void do_takeoff_help();
+
+	/**
+	 *	Update desired altitude base on user pitch stick input
+	 */
+	 void update_desired_altitude(float dt);
+
 	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
 					 const struct position_setpoint_triplet_s &_pos_sp_triplet);

@@ -454,6 +493,12 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),

 	_hold_alt(0.0f),
+	_ground_alt(0.0f),
+	_hdg_hold_yaw(0.0f),
+	_hdg_hold_enabled(false),
+	_yaw_lock_engaged(false),
+	_hdg_hold_prev_wp{},
+	_hdg_hold_curr_wp{},
 	_control_position_last_called(0),

 	land_noreturn_horizontal(false),
@@ -462,6 +507,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	land_useterrain(false),
+	_was_in_air(false),
+	_time_went_in_air(0),
 	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
@@ -855,6 +902,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c

 void FixedwingPositionControl::navigation_capabilities_publish()
 {
+	_nav_capabilities.timestamp = hrt_absolute_time();
+
 	if (_nav_capabilities_pub != nullptr) {
 		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
 	} else {
@@ -862,6 +911,31 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 	}
 }

+void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance,
+					struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init)
+{
+	waypoint_prev.valid = true;
+	waypoint_prev.alt = _hold_alt;
+
+	if (flag_init) {
+		// on init set first waypoint to momentary position
+		waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6;
+		waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6;
+	} else {
+		/*
+		for previous waypoint use the one still in front of us but shift it such that it is
+		located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us
+		*/
+		waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
+		waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
+	}
+
+	waypoint_next.valid = true;
+	waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
+	waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
+	waypoint_next.alt = _hold_alt;
+}
+
 float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
 {
 	if (!isfinite(global_pos.terrain_alt)) {
@@ -881,6 +955,41 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 	}
 }

+void FixedwingPositionControl::update_desired_altitude(float dt)
+{
+	const float deadBand = (60.0f/1000.0f);
+	const float factor = 1.0f - deadBand;
+	static bool was_in_deadband = false;
+
+	if (_manual.x > deadBand) {
+		float pitch = (_manual.x - deadBand) / factor;
+		_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
+		was_in_deadband = false;
+	} else if (_manual.x < - deadBand) {
+		float pitch = (_manual.x + deadBand) / factor;
+		_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
+		was_in_deadband = false;
+	} else if (!was_in_deadband) {
+		 /* store altitude at which manual.x was inside deadBand
+		  * The aircraft should immediately try to fly at this altitude
+		  * as this is what the pilot expects when he moves the stick to the center */
+		_hold_alt = _global_pos.alt;
+		was_in_deadband = true;
+	}
+}
+
+void FixedwingPositionControl::do_takeoff_help()
+{
+	const hrt_abstime delta_takeoff = 10000000;
+	const float throttle_threshold = 0.3f;
+	const float delta_alt_takeoff = 30.0f;
+
+	/* demand 30 m above ground if user switched into this mode during takeoff */
+	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) {
+		_hold_alt = _ground_alt + delta_alt_takeoff;
+	}
+}
+
 bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
@@ -912,6 +1021,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;

+	/* save time when airplane is in air */
+	if (!_was_in_air && !_vehicle_status.condition_landed) {
+		_was_in_air = true;
+		_time_went_in_air = hrt_absolute_time();
+		_ground_alt = _global_pos.alt;
+	}
+	/* reset flag when airplane landed */
+	if (_vehicle_status.condition_landed) {
+		_was_in_air = false;
+	}
+
 	if (_control_mode.flag_control_auto_enabled &&
 			pos_sp_triplet.current.valid) {
 		/* AUTONOMOUS FLIGHT */
@@ -928,6 +1048,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
+		/* reset hold yaw */
+		_hdg_hold_yaw = _att.yaw;

 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
@@ -1251,13 +1373,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 	} else if (_control_mode.flag_control_velocity_enabled &&
 			_control_mode.flag_control_altitude_enabled) {
-		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed,
+		   heading is set to a distant waypoint */

-		const float deadBand = (60.0f/1000.0f);
-		const float factor = 1.0f - deadBand;
 		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
+			_hdg_hold_yaw = _att.yaw;
+			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
+			_yaw_lock_engaged = false;
 		}
 		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
 		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
@@ -1274,30 +1398,118 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.z;

-		/* Get demanded vertical velocity from pitch control */
-		static bool was_in_deadband = false;
-		if (_manual.x > deadBand) {
-			float pitch = (_manual.x - deadBand) / factor;
-			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
-			was_in_deadband = false;
-		} else if (_manual.x < - deadBand) {
-			float pitch = (_manual.x + deadBand) / factor;
-			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
-			was_in_deadband = false;
-		} else if (!was_in_deadband) {
-			 /* store altitude at which manual.x was inside deadBand
-			  * The aircraft should immediately try to fly at this altitude
-			  * as this is what the pilot expects when he moves the stick to the center */
-			_hold_alt = _global_pos.alt;
-			was_in_deadband = true;
+		/* update desired altitude based on user pitch stick input */
+		update_desired_altitude(dt);
+
+		/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
+		do_takeoff_help();
+
+		/* throttle limiting */
+		throttle_max = _parameters.throttle_max;
+		if (fabsf(_manual.z) < THROTTLE_THRESH) {
+			throttle_max = 0.0f;
 		}
+
 		tecs_update_pitch_throttle(_hold_alt,
 				altctrl_airspeed,
 				eas2tas,
 				math::radians(_parameters.pitch_limit_min),
 				math::radians(_parameters.pitch_limit_max),
 				_parameters.throttle_min,
-				_parameters.throttle_max,
+				throttle_max,
+				_parameters.throttle_cruise,
+				false,
+				math::radians(_parameters.pitch_limit_min),
+				_global_pos.alt,
+				ground_speed,
+				tecs_status_s::TECS_MODE_NORMAL);
+
+		/* heading control */
+
+		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
+			/* heading / roll is zero, lock onto current heading */
+			if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
+				// little yaw movement, lock to current heading
+				_yaw_lock_engaged = true;
+
+			}
+
+			if (_yaw_lock_engaged) {
+
+				/* just switched back from non heading-hold to heading hold */
+				if (!_hdg_hold_enabled) {
+					_hdg_hold_enabled = true;
+					_hdg_hold_yaw = _att.yaw;
+
+					get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
+				}
+
+				/* we have a valid heading hold position, are we too close? */
+				if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
+					_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) {
+					get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
+				}
+
+				math::Vector<2> prev_wp;
+				prev_wp(0) = (float)_hdg_hold_prev_wp.lat;
+				prev_wp(1) = (float)_hdg_hold_prev_wp.lon;
+
+				math::Vector<2> curr_wp;
+				curr_wp(0) = (float)_hdg_hold_curr_wp.lat;
+				curr_wp(1) = (float)_hdg_hold_curr_wp.lon;
+
+				/* populate l1 control setpoint */
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+			}
+		} else {
+			_hdg_hold_enabled = false;
+			_yaw_lock_engaged = false;
+		}
+
+	} else if (_control_mode.flag_control_altitude_enabled) {
+		/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+
+		if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
+			/* Need to init because last loop iteration was in a different mode */
+			_hold_alt = _global_pos.alt;
+		}
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+		_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
+
+		/* Get demanded airspeed */
+		float altctrl_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.z;
+
+		/* update desired altitude based on user pitch stick input */
+		update_desired_altitude(dt);
+
+		/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
+		do_takeoff_help();
+
+		/* throttle limiting */
+		throttle_max = _parameters.throttle_max;
+		if (fabsf(_manual.z) < THROTTLE_THRESH) {
+			throttle_max = 0.0f;
+		}
+
+		tecs_update_pitch_throttle(_hold_alt,
+				altctrl_airspeed,
+				eas2tas,
+				math::radians(_parameters.pitch_limit_min),
+				math::radians(_parameters.pitch_limit_max),
+				_parameters.throttle_min,
+				throttle_max,
 				_parameters.throttle_cruise,
 				false,
 				math::radians(_parameters.pitch_limit_min),
@@ -1476,7 +1688,8 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);

 				/* lazily publish navigation capabilities */
-				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
+				if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON
+					&& turn_distance > 0)) {

 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;