px4-analyse-position [Digitale Elektronik und Technische Informatik]

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== Position Controller ======

===== Sourcecode =====
The relevant sourcecode for the position controller module is found [[https://github.com/PX4/Firmware/tree/v1.8.0/src/modules/fw_pos_control_l1|here]].


===== Analysis =====

==== Main-Loop ====

The main loop is executed whenever the ORB vehicle_global_position has been updated.

Parameters are update if the ORB parameter_update has been updated. The parameter update is handled in parameters_update(). 

Line 1662-1667\\
_global_pos and _local_pos are updated from the ORBS vehicle_global_position and vehicle_local_position.

Line 1668-1688\\
Some resets are performed if controller mode was switched to (partially) manual control.

Line 1689-1701\\
A number of ORB values is updated:
  * sensor_bias
  * airspeed
  * manual_control_setpoint
  * position_setpoint_triplet
  * vehicle_attitude
  * vehicle_control_mode
  * vehicle_land_detected
  * status_poll

The vectors for current position (curr_pos) and ground speed (ground_speed) are calculated using the global position data.

Line 1702-1706\\
A call to control_position() is executed. If sensors are not present or in manaul mode the rest of the main loop is skipped and nothing is done.

<code c++ FixedwingPositionControl.cpp>
control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current)
</code>

Line 1707-1712\\
A new timestamp is generated for the new attitude setpoint (_att_sp).
The offsets configured through parameters are added to roll and pitch.

Line 1713-1717\\
If control mode is (partially) manual roll and pitch are constraint to the configured parameters.

Line 1718-1721\\
The quaternion vector for the attitude setpoint is calculated from roll, pitch and yaw and set valid.

Line 1722-1737\\
If the control mode has offboard control disabled or position, velocity or acceleration control enabled then the attitude setpoint is published.

Line 1738-1761\\
The fixedwing position control status (ORB fw_pos_ctrl_status) is updated and published if a second has passed since the last time it was published. The most relevant value of this ORB seems to be the truning distance which specifies the optimal distance to a waypoint to switch to the next. This value is generated by calling _l1_control.switch_distance() in the external  [[https://github.com/PX4/ecl/tree/master/l1|l1_control library]] with a value of 500.

The l1_control_library returns the L1_distance if it does not exceed the passed value.

L1_ratio = 1 / PI * L1_damping * L1_period\\
L1_ration is about 4.77 with the default parameters provided (L1 damping = 0.75 and L1 period = 20).

L1_distance = L1_ratio * ground_speed\\
This means for a ground speed of 10 m/s the turning distance is 47 meters and for a ground speed of 20 m/s the turning distance would be 94m.

==== control_position() ====

<code c++ FixedwingPositionControl.hpp>
	bool		control_position(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
</code>

Line 697-705\\
Calculates the delta time since when the function was last called (default 0.01).

Line 706-715\\
Yaw will not be controlled with rudder and flaps will be disabled by default.\\
The _groundspeed_undershoot is calculated.

Line 711-726\\
The navigation speed vector nav_speed_2d is set using goundspeed. As a fallback it is set to airspeed in case the angle between airspeed and groundspeed exceeds 90 degrees or the ground speed is less than 3 m/s. This is done to prevent errors in the l1 navigation module that cannot handle wind speed exceeding maximum airspeed.\\
Initial calculations are done with no throttle limit.

Line 730-741\\
In case the airplane has just taken off (!_vehicle_land_detected.landed) the variables for _time_went_in_air and _takeoff_ground_alt are updated. Altitude is taken from the global position (_global_pos).

Line 742-147\\
Integrators of TECS are reset if switching to this mode from another mode (FW_POSCTRL_MODE_OTHER) in which posctl was not active.

**Line 748-883 describe autonomous flight mode**\\
_hold_alt is reset to the current global position altitude.
_hdg_hold_yaw is reset to current yaw.

The speedweigt for TECS is reset to the current parameter value for speed weight.

The current waypoint (curr_wp) is generated from the passed pos_sp_curr.

The roll, pitch and yaw integral is taken out of reset for the attitude setpoint (_att_sp).

The previous waypoint (prev_wp) is taken from the passed pos_sp_prev if it is valid. If it is not valid previous waypoint is set to the current waypoint.

Mission airspeed (mission_airspeed) is initially set to the airspeed trim parameter (FW_AIRSPD_TRIM). If the passed position setpoint comes with a valid cruising speed this value is taken instead.

Mission throttle (mission_throttle) is set tot the throttle_cruise parameter value (FW_THR_CRUISE). If the passed position setpoint comes with a valid cruising throttle this value is taken instead.

If the current position setpoint type (pos_sp_curr.type) is SETPOINT_TYPE_IDLE thrust, roll and pitch for the attitude setpoint are set to 0.

If the current position setpoint type is SETPOINT_TYPE_POSITION:
<code c++ FixedwingPositionControl.cpp>
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
</code>

Afterwards roll and yaw of the attitude setpoint are set to the return values of the _l1_control functions nav_roll() and nav_bearing(). tecs_update_pitch_throttle is called with the following parameters:

<code c++ FixedwingPositionControl.cpp>
			tecs_update_pitch_throttle(pos_sp_curr.alt,
						   calculate_target_airspeed(mission_airspeed),
						   radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad,
						   radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad,
						   _parameters.throttle_min,
						   _parameters.throttle_max,
						   mission_throttle,
						   false,
						   radians(_parameters.pitch_limit_min));
</code>

If the current position setpoint type is SETPOINT_TYPE_LOITER:
<code c++ FixedwingPositionControl.cpp>
_l1_control.navigate_loiter(curr_wp, curr_pos, pos_sp_curr.loiter_radius, pos_sp_curr.loiter_direction, nav_speed_2d);
</code>

Afterwards roll and yaw of the attitude setpoint are set to the return values of the _l1_control functions nav_roll() and nav_bearing(). 

Altitude setpoint (alt_sp) is set to pos_sp_curr.alt.

If the plane is in a takeoff situation altitude setpoint is set to the _takeoff_ground_alt + _parameters.climbout_diff if that value is heigher than the altitude setpoint. Additionally the roll of the attitude setpoint is limited to -5 to 5 degrees.

Aborting an ongoing landing is handled by setting  the roll of the attitude setpoint to 0 if the height difference to the target waypoint does not exceed the parameter climbeout difference.


tecs_update_pitch_throttle is called with the following parameters:

<code c++ FixedwingPositionControl.cpp>
			tecs_update_pitch_throttle(alt_sp,
						   calculate_target_airspeed(mission_airspeed),
						   radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad,
						   radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad,
						   _parameters.throttle_min,
						   _parameters.throttle_max,
						   _parameters.throttle_cruise,
						   false,
						   radians(_parameters.pitch_limit_min));
</code>

If the current position setpoint type is SETPOINT_TYPE_LAND then control_landing() is called otherwise the landing state is reset by calling reset_landing_state().

If the current position setpoint type is SETPOINT_TYPE_TAKEOFF control_takeoff() is called otherwise the takeoff state is reset by calling reset_takeoff_state().

If this is the first time l1_control changed its state from circle_mode to non circle_mode the roll_reset_integral in the attitude setpoint is set to true.


**Line 884-994 describe position control mode**\\
Not examined yet

**Line 995-1037 describe altitude control mode**\\
Not examined yet

Line 1038-1053 handles other modes\\
The function will return false.
Hold altitude is set to global position altitude.
If control mode (_control_mode) was manual last time the function was called reset_landing_state() and reset_takeoff_state() are called.

Line 1054 - 1117\\
The attitude setpoint pitch is overwritten with the pitch from tecs. This is prevented if the plane is in auto runway takeoff, flaring during landing or in manual attitude cotnrol mode.



==== vehicle attitude setpoint ====

The vehicle attitude setpoint contains the following information:

<code c++ vehicle_attitude_setpoint.msg>
int8 LANDING_GEAR_UP = 1	# landing gear up
int8 LANDING_GEAR_DOWN = -1	# landing gear down

float32 roll_body				# body angle in NED frame
float32 pitch_body				# body angle in NED frame
float32 yaw_body				# body angle in NED frame

float32 yaw_sp_move_rate		# rad/s (commanded by user)

# For quaternion-based attitude control
float32[4] q_d					# Desired quaternion for quaternion control
bool q_d_valid					# Set to true if quaternion vector is valid

float32 thrust					# Thrust in Newton the power system should generate

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_yaw					# control heading with rudder (used for auto takeoff on runway)
bool disable_mc_yaw_control			# control yaw for mc (used for vtol weather-vane mode)

bool apply_flaps

float32 landing_gear
</code>

==== fixedwing position control status ====

The fixedwing position control status contains the following information:

<code c++ fw_pos_ctrl_status.msg>
float32 nav_roll
float32 nav_pitch
float32 nav_bearing

float32 target_bearing
float32 wp_dist
float32 xtrack_error
float32 turn_distance		# the optimal distance to a waypoint to switch to the next

float32 landing_horizontal_slope_displacement
float32 landing_slope_angle_rad
float32 landing_flare_length
bool abort_landing
</code>

==== parameters_update() ====

The following parameters are updated:

<code c++ FixedwingPositionControl.cpp>
	struct {
		float climbout_diff;

		float max_climb_rate;
		float max_sink_rate;
		float speed_weight;

		float airspeed_min;
		float airspeed_trim;
		float airspeed_max;
		int32_t airspeed_disabled;

		float pitch_limit_min;
		float pitch_limit_max;

		float throttle_min;
		float throttle_max;
		float throttle_idle;
		float throttle_cruise;
		float throttle_alt_scale;

		float man_roll_max_rad;
		float man_pitch_max_rad;
		float rollsp_offset_rad;
		float pitchsp_offset_rad;

		float throttle_land_max;

		float land_heading_hold_horizontal_distance;
		float land_flare_pitch_min_deg;
		float land_flare_pitch_max_deg;
		int32_t land_use_terrain_estimate;
		float land_airspeed_scale;

		// VTOL
		float airspeed_trans;
		int32_t vtol_type;
	} _parameters{};
</code>

Beside the locally used parameters updating TECS parameters is handled here:

<code c++ FixedwingPositionControl.cpp>
	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
	_tecs.set_speed_weight(_parameters.speed_weight);
	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
	_tecs.set_time_const(v);
	_tecs.set_time_const_throt(v);
	_tecs.set_min_sink_rate(v);
	_tecs.set_throttle_damp(v);
	_tecs.set_integrator_gain(v);
	_tecs.set_throttle_slewrate(v);
	_tecs.set_vertical_accel_limit(v);
	_tecs.set_height_comp_filter_omega(v);
	_tecs.set_speed_comp_filter_omega(v);
	_tecs.set_roll_throttle_compensation(v);
	_tecs.set_pitch_damping(v);
	_tecs.set_heightrate_p(v);
	_tecs.set_heightrate_ff(v);
	_tecs.set_speedrate_p(v);

</code>

Parameters relate to the landing slope are updated.

The landing slope related values of the ORB fw_pos_ctrl_status are updated and published.

A sanity check is performed for airspeed parameters.


==== calculate_target_airspeed() ====
<code c++ FixedwingPositionControl.hpp>
float		calculate_target_airspeed(float airspeed_demand);
</code>
A previously calculated _groundspeed_undershoot value is added to the airspeed. The returned airspeed is constrained to a value between FW_AIRSPD_MAX and an increased FW_AIRSPD_MIN. FW_AIRSPEED_MIN is increased to ensure the increased demand for lift if the plane (current _att_sp) has a roll angle is met. This increase is only applied if airspeed is vaild (_airspeed_valid). 

==== calculate_gndspeed_undershoot() ====

<code c++ FixedwingPositionControl.hpp>
	void		calculate_gndspeed_undershoot(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
</code>
The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available. _groundspeed_undershoot ensures that a plane (as long as its throttle capability is not exceeded) travels towards a waypoint (and is not pushed more and more away by wind). Not countering this would lead to a fly-away.

==== get_waypoint_heading_distance() ====
<code c++ FixedwingPositionControl.hpp>
static constexpr float HDG_HOLD_DIST_NEXT =	3000.0f; // initial distance of waypoint in front of plane in heading hold mode
static constexpr float HDG_HOLD_REACHED_DIST =	1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
	void		get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, position_setpoint_s &waypoint_next, bool flag_init);
</code>
Generates two waypoints based on the heading parameter and current position (if flag_init is true). The waypoint_prev is generated 100 meters behind the plane and the waypoint_next is generated 3000 meters ahead of the plane. \\ 
If flag_init is false the passed waypoint_next and waypoint_prev are used to generate a line. The waypoint_prev is generated on the line 1100 meters behind waypoint_next and the new waypoint_next is generated on the line 4000 meters ahead of the old waypoint_next.\\
_hold_alt is used for all waypoints.

==== update_desired_altitude() ====
<code c++ FixedwingPositionControl.hpp>
	bool		update_desired_altitude(float dt);
</code>
Update desired altitude base on user pitch stick input based on _manual.x (from manual_control_setpoint) and a dt (delta time) argument.

==== handle_command() ====
This funcion solely handles the VEHICLE_CMD_DO_GO_AROUND.
It  abort landing before point of no return (horizontal and vertical) by setting _fw_pos_ctrl_status.abort_landing to true.


==== airspeed_poll() ====

Updates the airspeed (_airspeed) to the indicated airspeed published (ORB airspeed).
If there was no airspeed update for more then a seconde _airspeed_valid is set to false.

If the true airspeed is larger than the indicated airspeed _eas2tas is updated which is used when calling _tecs.update_pitch_throttle().

TECS is notified if airspeed is still valid by calling _tecs.enable_airspeed(_airspeed_valid).

==== control_takeoff() ====
Not examined yet
==== control_landing() ====
Not examined yet
==== tecs_update_pitch_throttle() ====
Not examined yet

===== Copyright =====

The following copyright applies to all sourcecode shown on this page:
<code c++>
/****************************************************************************
 *
 *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
</code>