Merged release_v1.0.0 branch into master

2026-10-28 04:13:33 +08:00 · 2015-06-25 21:45:17 +02:00
parent 74bdec2cc6 5b93d92339
commit 454becdae5
73 changed files with 2726 additions and 887 deletions
@@ -98,10 +98,12 @@ static int	_control_task = -1;			/**< task handle for sensor task */
 #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
+#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll input from user which does not change the locked heading
+#define TAKEOFF_IDLE				0.2f 		// idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0)

+static constexpr float THROTTLE_THRESH = 0.05f; 	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
+static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f;	///< a throttle / pitch input above this value leads to the system switching to climbout mode
+static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;

 /**
 * L1 control app start / stop handling function
@@ -173,10 +175,10 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */

 	float	_hold_alt;				/**< hold altitude for altitude mode */
-	float 	_ground_alt;			/**< ground altitude at which plane was launched */
+	float	_takeoff_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 */
+	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  */
@@ -370,7 +372,7 @@ private:
 	/**
 	 * Publish navigation capabilities
 	 */
-	void navigation_capabilities_publish();
+	void		navigation_capabilities_publish();

 	/**
 	 * Get a new waypoint based on heading and distance from current position
@@ -386,27 +388,36 @@ private:
 	/**
 	 * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
 	 */
-	float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
+	float		get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
+
+	/**
+	 * Check if we are in a takeoff situation
+	 */
+	bool in_takeoff_situation();
+
+	/**
+	 * Do takeoff help when in altitude controlled modes
+	 * @param hold_altitude altitude setpoint for controller
+	 * @param pitch_limit_min minimum pitch allowed
+	 */
+	void 		do_takeoff_help(float *hold_altitude, float *pitch_limit_min);
+
+	/**
+	 * Update desired altitude base on user pitch stick input
+	 *
+	 * @param dt Time step
+	 * @return true if climbout mode was requested by user (climb with max rate and min airspeed)
+	 */
+	bool		update_desired_altitude(float dt);

 	/**
 	 * 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);

-	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
+	float		calculate_target_airspeed(float airspeed_demand);
+	void		calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);

 	/**
 	 * Shim for calling task_main from task_create.
@@ -421,12 +432,12 @@ private:
 	/*
 	 * Reset takeoff state
 	 */
-	void reset_takeoff_state();
+	void		reset_takeoff_state();

 	/*
 	 * Reset landing state
 	 */
-	void reset_landing_state();
+	void		reset_landing_state();

 	/*
 	 * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
@@ -493,7 +504,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),

 	_hold_alt(0.0f),
-	_ground_alt(0.0f),
+	_takeoff_ground_alt(0.0f),
 	_hdg_hold_yaw(0.0f),
 	_hdg_hold_enabled(false),
 	_yaw_lock_engaged(false),
@@ -955,16 +966,35 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 	}
 }

-void FixedwingPositionControl::update_desired_altitude(float dt)
+bool FixedwingPositionControl::update_desired_altitude(float dt)
 {
 	const float deadBand = (60.0f/1000.0f);
 	const float factor = 1.0f - deadBand;
+	// XXX this should go into a manual stick mapper
+	// class
+	static float _althold_epv = 0.0f;
 	static bool was_in_deadband = false;
+	bool climbout_mode = false;
+
+	/*
+	 * Reset the hold altitude to the current altitude if the uncertainty
+	 * changes significantly.
+	 * This is to guard against uncommanded altitude changes
+	 * when the altitude certainty increases or decreases.
+	 */
+
+	if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) {
+		_hold_alt = _global_pos.alt;
+		_althold_epv = _global_pos.epv;
+	}
+
+	// XXX the sign magic in this function needs to be fixed

 	if (_manual.x > deadBand) {
 		float pitch = (_manual.x - deadBand) / factor;
 		_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
 		was_in_deadband = false;
+		climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH);
 	} else if (_manual.x < - deadBand) {
 		float pitch = (_manual.x + deadBand) / factor;
 		_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
@@ -974,19 +1004,32 @@ void FixedwingPositionControl::update_desired_altitude(float dt)
 		  * 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;
+		_althold_epv = _global_pos.epv;
 		was_in_deadband = true;
 	}
+
+	return climbout_mode;
 }

-void FixedwingPositionControl::do_takeoff_help()
-{
+bool FixedwingPositionControl::in_takeoff_situation() {
 	const hrt_abstime delta_takeoff = 10000000;
-	const float throttle_threshold = 0.3f;
-	const float delta_alt_takeoff = 30.0f;
+	const float throttle_threshold = 0.1f;

-	/* 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;
+	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) {
+		return true;
+	}
+
+	return false;
+}
+
+void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min)
+{
+	/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
+	if (in_takeoff_situation()) {
+		*hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff;
+		*pitch_limit_min = math::radians(10.0f);
+	} else {
+		*pitch_limit_min = _parameters.pitch_limit_min;
 	}
 }

@@ -1011,7 +1054,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;

-	if (!_mTecs.getEnabled()) {
+	if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
 		_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
 	}

@@ -1025,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	if (!_was_in_air && !_vehicle_status.condition_landed) {
 		_was_in_air = true;
 		_time_went_in_air = hrt_absolute_time();
-		_ground_alt = _global_pos.alt;
+		_takeoff_ground_alt = _global_pos.alt;
 	}
 	/* reset flag when airplane landed */
 	if (_vehicle_status.condition_landed) {
@@ -1085,7 +1128,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 		}

-		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+			_att_sp.thrust = 0.0f;
+			_att_sp.roll_body = 0.0f;
+			_att_sp.pitch_body = 0.0f;
+
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -1104,6 +1152,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();

+			if (in_takeoff_situation()) {
+					/* limit roll motion to ensure enough lift */
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
+							math::radians(15.0f));
+			}
+
 			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1270,7 +1324,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* Inform user that launchdetection is running */
 				static hrt_abstime last_sent = 0;
 				if(hrt_absolute_time() - last_sent > 4e6) {
-					mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
+					mavlink_log_critical(_mavlink_fd, "Launchdetection running");
 					last_sent = hrt_absolute_time();
 				}

@@ -1399,10 +1453,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					  _manual.z;

 		/* update desired altitude based on user pitch stick input */
-		update_desired_altitude(dt);
+		bool climbout_requested = update_desired_altitude(dt);
+
+		/* if we assume that user is taking off then help by demanding altitude setpoint well above ground 
+		* and set limit to pitch angle to prevent stearing into ground
+		*/
+		float pitch_limit_min;
+		do_takeoff_help(&_hold_alt, &pitch_limit_min);

-		/* 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;
@@ -1418,8 +1476,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				_parameters.throttle_min,
 				throttle_max,
 				_parameters.throttle_cruise,
-				false,
-				math::radians(_parameters.pitch_limit_min),
+				climbout_requested,
+				pitch_limit_min,
 				_global_pos.alt,
 				ground_speed,
 				tecs_status_s::TECS_MODE_NORMAL);
@@ -1434,6 +1492,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 			}

+			/* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration
+			  to make sure the plane does not start rolling
+			*/
+			if (in_takeoff_situation()) {
+				_hdg_hold_enabled = false;
+				_yaw_lock_engaged = true;
+			}
+
 			if (_yaw_lock_engaged) {

 				/* just switched back from non heading-hold to heading hold */
@@ -1463,6 +1529,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				if (in_takeoff_situation()) {
+					/* limit roll motion to ensure enough lift */
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
+							math::radians(15.0f));
+				}
 			}
 		} else {
 			_hdg_hold_enabled = false;
@@ -1492,10 +1564,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					  _manual.z;

 		/* update desired altitude based on user pitch stick input */
-		update_desired_altitude(dt);
+		bool climbout_requested = update_desired_altitude(dt);

-		/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
-		do_takeoff_help();
+		/* if we assume that user is taking off then help by demanding altitude setpoint well above ground 
+		* and set limit to pitch angle to prevent stearing into ground
+		*/
+		float pitch_limit_min;
+		do_takeoff_help(&_hold_alt, &pitch_limit_min);

 		/* throttle limiting */
 		throttle_max = _parameters.throttle_max;
@@ -1511,8 +1586,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				_parameters.throttle_min,
 				throttle_max,
 				_parameters.throttle_cruise,
-				false,
-				math::radians(_parameters.pitch_limit_min),
+				climbout_requested,
+				pitch_limit_min,
 				_global_pos.alt,
 				ground_speed,
 				tecs_status_s::TECS_MODE_NORMAL);
@@ -1544,10 +1619,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		_att_sp.thrust = 0.0f;
 	} else {
 		/* Copy thrust and pitch values from tecs */
-		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
-				_tecs.get_throttle_demand(), throttle_max);
+		if (_vehicle_status.condition_landed &&
+			(_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE))
+		{
+			// when we are landed in these modes we want the motor to spin
+			_att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max);
+		} else {
+			_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
+					_tecs.get_throttle_demand(), throttle_max);
+		}
+
+
 	}

 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
@@ -1736,6 +1823,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		const math::Vector<3> &ground_speed,
 		unsigned mode, bool pitch_max_special)
 {
+	/* do not run tecs if we are not in air */
+	if (_vehicle_status.condition_landed) {
+		return;
+	}
+
 	if (_mTecs.getEnabled()) {
 		/* Using mtecs library: prepare arguments for mtecs call */
 		float flightPathAngle = 0.0f;