Differences

This shows you the differences between two versions of the page.

--- px4-analyse-position [2018/10/26 20:21]
natwati [handle_command()]
+++ px4-analyse-position [2018/10/27 22:02] (current)
natwati
@@ Line 34: / Line 34: @@
 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\\
@@ Line 58: / Line 62: @@
 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 ====
@@ Line 188: / Line 303: @@
-==== Other Functions ====
+==== 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 plane (current _att_sp) has a roll angle is met.
+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>
@@ Line 199: / Line 316: @@
 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
@@ Line 208: / Line 326: @@
 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);
@@ Line 213: / Line 333: @@
 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() ====
+==== 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.
-<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\\
+==== airspeed_poll() ====
-Calculates the delta time since when the function was last called (default 0.01).
-Line 706-715\\
+Updates the airspeed (_airspeed) to the indicated airspeed published (ORB airspeed).
-Yaw will not be controlled with rudder and flaps will be disabled by default.\\
+If there was no airspeed update for more then a seconde _airspeed_valid is set to false.
-The _groundspeed_undershoot is calculated.
-Line 711-726\\
+If the true airspeed is larger than the indicated airspeed _eas2tas is updated which is used when calling _tecs.update_pitch_throttle().
-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\\
+TECS is notified if airspeed is still valid by calling _tecs.enable_airspeed(_airspeed_valid).
-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.
-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() ====
@@ Line 331: / Line 351: @@
 ==== control_landing() ====
 Not examined yet
-==== handle_command() ====
+==== tecs_update_pitch_throttle() ====
 Not examined yet
-==== tecs_update_pitch_throttle() ====
-TODO
-==== airspeed_poll() ====
-TODO
 ===== Copyright =====
 The following copyright applies to all sourcecode shown on this page: