fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 18:24:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw autoland: add parameter for heading hold distance, fix heading hold

Browse Source
This commit is contained in:
Thomas Gubler 2013-12-27 20:15:54 +01:00
parent 9684367c8f
commit b98984e1ff
2 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -49,6 +49,7 @@
* More details and acknowledgements in the referenced library headers.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@ -227,6 +228,7 @@ private:
float land_H1_virt;
float land_flare_alt_relative;
float land_thrust_lim_horizontal_distance;
float land_heading_hold_horizontal_distance;
} _parameters; /**< local copies of interesting parameters */
@ -271,6 +273,7 @@ private:
param_t land_H1_virt;
param_t land_flare_alt_relative;
param_t land_thrust_lim_horizontal_distance;
param_t land_heading_hold_horizontal_distance;
} _parameter_handles; /**< handles for interesting parameters */
@ -420,6 +423,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@ -508,6 +512,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
@ -822,19 +827,18 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY());
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
const float heading_hold_distance = 15.0f;
if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
/* heading hold, along the line connecting this and the last waypoint */
// if (mission_item_triplet.previous_valid) {
// target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
// } else {
if (!land_noreturn_horizontal) //set target_bearing in first occurrence
target_bearing = _att.yaw;
//}
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
if (mission_item_triplet.previous_valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY());
} else {
target_bearing = _att.yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));

1
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -120,3 +120,4 @@ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);