fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 07:33:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+14 -10
View File
@@ -49,6 +49,7 @@
* More details and acknowledgements in the referenced library headers. * More details and acknowledgements in the referenced library headers.
* *
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@@ -227,6 +228,7 @@ private:
float land_H1_virt; float land_H1_virt;
float land_flare_alt_relative; float land_flare_alt_relative;
float land_thrust_lim_horizontal_distance; float land_thrust_lim_horizontal_distance;
float land_heading_hold_horizontal_distance;
} _parameters; /**< local copies of interesting parameters */ } _parameters; /**< local copies of interesting parameters */
@@ -271,6 +273,7 @@ private:
param_t land_H1_virt; param_t land_H1_virt;
param_t land_flare_alt_relative; param_t land_flare_alt_relative;
param_t land_thrust_lim_horizontal_distance; param_t land_thrust_lim_horizontal_distance;
param_t land_heading_hold_horizontal_distance;
} _parameter_handles; /**< handles for interesting parameters */ } _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_H1_virt = param_find("FW_LND_HVIRT");
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _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_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.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _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_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); 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_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_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period); _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 */ /* 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()); 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"); //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 < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
/* heading hold, along the line connecting this and the last waypoint */ /* heading hold, along the line connecting this and the last waypoint */
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
// if (mission_item_triplet.previous_valid) { 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()); target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY());
// } else { } else {
target_bearing = _att.yaw;
if (!land_noreturn_horizontal) //set target_bearing in first occurrence }
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)); // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+1
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_HVIRT, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);