This is an old revision of the document!
Position Controller
Sourcecode
The relevant sourcecode for the position controller module is found .
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.
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 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.
vehicle attitude setpoint
The vehicle attitude setpoint contains the following information:
- 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
fixedwing position control status
The fixedwing position control status contains the following information:
- 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
parameters_update()
The following parameters are updated:
- 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{};
Beside the locally used parameters updating TECS parameters is handled here:
- 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);
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.
Other Functions
- FixedwingPositionControl.hpp
float calculate_target_airspeed(float airspeed_demand);
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 plane (current _att_sp) has a roll angle is met.
- 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);
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.
- 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);
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.
- FixedwingPositionControl.hpp
bool update_desired_altitude(float dt);
Update desired altitude base on user pitch stick input based on _manual.x (from manual_control_setpoint) and a dt (delta time) argument.
control_position()
- 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);
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.
Line 884-994 describe position control mode
Line 995-1037 describe altitude control mode
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
Line 1699 Update curr_pos and ground_speed based on current _global_pos (global vehicle position). Call to control_position() with current position, ground speed and the previous and current setpoint of the position triplet.
control_takeoff()
TODO
control_landing()
TODO
handle_command()
TODO
tecs_update_pitch_throttle()
TODO
airspeed_poll()
TODO
Copyright
The following copyright applies to all sourcecode shown on this page:
/*
*
* 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.
*
*/