From 4a66e285adcd88cf42c3907753abd9b433815e45 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 00:05:27 +0100 Subject: [PATCH 01/10] navigator mavlink log info messages: add #audio tag --- src/modules/navigator/navigator_main.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..2711be24fc 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,7 +865,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] nav state: %s", nav_states_str[myState]); prevState = myState; /* reset time counter on state changes */ @@ -1073,11 +1073,11 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter at current altitude"); } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; @@ -1175,14 +1175,14 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to offboard WP %d", index); } } @@ -1331,7 +1331,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1357,7 +1357,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1384,12 +1384,12 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); break; } @@ -1516,7 +1516,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } @@ -1541,7 +1541,7 @@ Navigator::on_mission_item_reached() if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff completed"); } else { /* advance by one mission item */ From 6480c6a2cee7a237f9310f6986567e91c54c1247 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 21:48:54 +0100 Subject: [PATCH 02/10] fw autoland: remove confusing slopelength parameter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +----- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 ------- 2 files changed, 1 insertion(+), 12 deletions(-) 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 45fdaa355e..020d1c119e 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 @@ -233,7 +233,6 @@ private: float speedrate_p; float land_slope_angle; - float land_slope_length; float land_H1_virt; float land_flare_alt_relative; float land_thrust_lim_alt_relative; @@ -278,7 +277,6 @@ private: param_t speedrate_p; param_t land_slope_angle; - param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; @@ -431,7 +429,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _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_alt_relative = param_find("FW_LND_TLALT"); @@ -520,7 +517,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); - param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); 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_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); @@ -889,7 +885,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * 0.9f; float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); 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 62a340e905..0909135e15 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 @@ -348,13 +348,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); */ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); -/** - * Landing slope length - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); - /** * * From b7488da441cf945ec1e5f0a1a4c4b6295707a8b0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 22:06:05 +0100 Subject: [PATCH 03/10] fw autoland: completely remove the virtual wp between the last wp and the landing wp. autoland starts now from the last normal wp. --- .../fw_pos_control_l1_main.cpp | 32 +++++++------------ 1 file changed, 11 insertions(+), 21 deletions(-) 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 020d1c119e..e1869328a1 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 @@ -885,7 +885,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * 0.9f; + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); @@ -931,34 +931,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_last = flare_curve_alt; - - } else if (wp_distance < L_wp_distance) { - - /* minimize speed to approach speed, stay on landing slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); - - if (!land_onslope) { - - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); - land_onslope = true; - } - } else { /* intersect glide slope: - * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope - * */ + * minimize speed to approach speed + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + + if (!land_onslope) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + land_onslope = true; + } + } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); From 9830f668c548ed63131722cdbba4753e1c06f647 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 22:55:40 +0100 Subject: [PATCH 04/10] fw autoland: fix warning --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e1869328a1..b0d2bcbc6d 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 @@ -912,7 +912,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) { flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; From 60de15ea9d753eef3361778633f7f91fb92aa748 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:26:50 +0100 Subject: [PATCH 05/10] fw autoland: remove old comments --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ---- 1 file changed, 4 deletions(-) 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 b0d2bcbc6d..3367aee9aa 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 @@ -942,17 +942,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } - } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); - //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), From b7899056f5454d8508a20dff87661c424ad98340 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:32:55 +0100 Subject: [PATCH 06/10] fw autoland: add very short comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 3367aee9aa..64d2b70197 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 @@ -885,8 +885,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); From f4bac7e7e802abf671d0ee87ad84b6f281fd81be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 21 Feb 2014 20:22:05 +0100 Subject: [PATCH 07/10] navigator: remove all but one [navigator] tag from mavlink audio messages --- src/modules/navigator/navigator_main.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2711be24fc..0268c47e73 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,7 +865,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] nav state: %s", nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); prevState = myState; /* reset time counter on state changes */ @@ -1073,11 +1073,11 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter at current altitude"); + mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; @@ -1175,14 +1175,14 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to onboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); } } @@ -1331,7 +1331,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1357,7 +1357,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1384,7 +1384,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1516,7 +1516,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", _mission_item.time_inside); } } @@ -1541,7 +1541,7 @@ Navigator::on_mission_item_reached() if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff completed"); + mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); } else { /* advance by one mission item */ From ab8ece3961f50348cf4bf464cb7d953b182a0a44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 12:42:48 +0100 Subject: [PATCH 08/10] l1_pos_control: prevent NaNs for roll setpoint --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 3b68a0a4e8..d1c864d782 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -38,6 +38,8 @@ * */ +#include + #include "ecl_l1_pos_controller.h" float ECL_L1_Pos_Controller::nav_roll() @@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con /* calculate the vector from waypoint A to current position */ math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - /* store the normalized vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit; + + /* prevent NaN when normalizing */ + if (vector_A_to_airplane.length() > FLT_EPSILON) { + /* store the normalized vector from waypoint A to current position */ + vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + } else { + vector_A_to_airplane_unit = vector_A_to_airplane; + } /* calculate eta angle towards the loiter center */ From 75c6706278119e50ce56ecbb1620025a9b9b936d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:19:57 +0100 Subject: [PATCH 09/10] l1_pos_control: set gndspeed_undershoot to 0 in loiter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ed57e0d349..2e8d038d0d 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 @@ -695,7 +695,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid) { + if (_global_pos_valid && !pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); From 909e138182000350df19d47367d6f68d0a6b7fd4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:27:24 +0100 Subject: [PATCH 10/10] l1_pos_control: fix stupid 'if not' mistake --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2e8d038d0d..ed6d8792ce 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 @@ -695,7 +695,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));