From f8c2ee73db3654a34976291e5ac94c437e27d4a7 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 11 Jan 2022 14:10:59 +0100 Subject: [PATCH] handle line segment termination in navigator - if following line segment (fixed-wing position control) switch waypoint when in acceptance radius OR passed the second waypoint. this handles the case of being beyond the second waypoint but not within the acceptance radius without the need to fly back to the waypoint (e.g. after a loiter up to waypoint alt) - sync navigator and fw pos ctrl waypoint acceptance altitude --- .../FixedwingPositionControl.cpp | 2 +- .../FixedwingPositionControl.hpp | 4 ++- src/modules/navigator/mission_block.cpp | 36 +++++++++++++++++-- 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 4119ecde37..b85d8b711e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -994,7 +994,7 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons // POSITION: achieve position setpoint altitude via loiter // close to waypoint, but altitude error greater than twice acceptance if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f) - && (dist_z > 2.f * _param_fw_clmbout_diff.get()) + && (dist_z > _param_nav_fw_alt_rad.get()) && (dist_xy < 2.f * math::max(acc_rad, loiter_radius_abs))) { // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 27c104258e..4ffb648876 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -447,7 +447,9 @@ private: (ParamFloat) _param_nav_loiter_rad, - (ParamFloat) _takeoff_pitch_min + (ParamFloat) _takeoff_pitch_min, + + (ParamFloat) _param_nav_fw_alt_rad ) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2ba4e854d5..f75a2700ed 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -307,8 +307,40 @@ MissionBlock::is_mission_item_reached() } - if (dist_xy >= 0.0f && dist_xy <= acceptance_radius - && dist_z <= alt_acc_rad_m) { + bool passed_curr_wp = false; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + const float dist_prev_to_curr = get_distance_to_next_waypoint(_navigator->get_position_setpoint_triplet()->previous.lat, + _navigator->get_position_setpoint_triplet()->previous.lon, _navigator->get_position_setpoint_triplet()->current.lat, + _navigator->get_position_setpoint_triplet()->current.lon); + + if (dist_prev_to_curr > 1.0e-6f && _navigator->get_position_setpoint_triplet()->previous.valid) { + // Fixed-wing guidance interprets this condition as line segment following + + // vector from previous waypoint to current waypoint + float vector_prev_to_curr_north; + float vector_prev_to_curr_east; + get_vector_to_next_waypoint_fast(_navigator->get_position_setpoint_triplet()->previous.lat, + _navigator->get_position_setpoint_triplet()->previous.lon, _navigator->get_position_setpoint_triplet()->current.lat, + _navigator->get_position_setpoint_triplet()->current.lon, &vector_prev_to_curr_north, + &vector_prev_to_curr_east); + + // vector from next waypoint to aircraft + float vector_curr_to_vehicle_north; + float vector_curr_to_vehicle_east; + get_vector_to_next_waypoint_fast(_navigator->get_position_setpoint_triplet()->current.lat, + _navigator->get_position_setpoint_triplet()->current.lon, _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, &vector_curr_to_vehicle_north, + &vector_curr_to_vehicle_east); + + // if dot product of vectors is positive, we are passed the current waypoint (the terminal point on the line segment) and should switch to next mission item + passed_curr_wp = vector_prev_to_curr_north * vector_curr_to_vehicle_north + vector_prev_to_curr_east * + vector_curr_to_vehicle_east > 0.0f; + } + } + + if (dist_xy >= 0.0f && (dist_xy <= acceptance_radius || passed_curr_wp) && dist_z <= alt_acc_rad_m) { _waypoint_position_reached = true; } }