From b98984e1ffa924d0888efa47af046d1127e9addf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 27 Dec 2013 20:15:54 +0100 Subject: fw autoland: add parameter for heading hold distance, fix heading hold --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 24 +++++++++++++--------- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 1 + 2 files changed, 15 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3784337ac..ee0aca69e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,6 +49,7 @@ * More details and acknowledgements in the referenced library headers. * * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -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 ¤t_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)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 10a0c00fc..9f46b5170 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -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); -- cgit v1.2.3