diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index bd77ed7f77..30d3b8b0e7 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -469,7 +469,7 @@ void Plane::update_navigation() case Mode::Number::RTL: if (quadplane.available() && quadplane.rtl_mode == 1 && (nav_controller->reached_loiter_target() || - location_passed_point(current_loc, prev_WP_loc, next_WP_loc) || + current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc) || auto_state.wp_distance < MAX(qrtl_radius, quadplane.stopping_distance())) && AP_HAL::millis() - last_mode_change_ms > 1000) { /* @@ -588,7 +588,7 @@ void Plane::update_alt() if (auto_throttle_mode && !throttle_suppressed) { float distance_beyond_land_wp = 0; - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { + if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { distance_beyond_land_wp = current_loc.get_distance(next_WP_loc); } diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index c9d0013433..de37527966 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -45,7 +45,7 @@ void Plane::adjust_altitude_target() // altitude target set_target_altitude_location(next_WP_loc); } else if (target_altitude.offset_cm != 0 && - !location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { + !current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { // control climb/descent rate set_target_altitude_proportion(next_WP_loc, 1.0f-auto_state.wp_proportion); @@ -66,8 +66,7 @@ void Plane::setup_glide_slope(void) // establish the distance we are travelling to the next waypoint, // for calculating out rate of change of altitude auto_state.wp_distance = current_loc.get_distance(next_WP_loc); - auto_state.wp_proportion = location_path_proportion(current_loc, - prev_WP_loc, next_WP_loc); + auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc); SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion); update_flight_stage(); diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index df8909ca38..3d388b3da6 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -50,7 +50,7 @@ void Plane::set_next_WP(const struct Location &loc) // past the waypoint when we start on a leg, then use the current // location as the previous waypoint, to prevent immediately // considering the waypoint complete - if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { + if (current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint"); prev_WP_loc = current_loc; } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 8c0e9f7e21..2305feec69 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -617,7 +617,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // If override with p3 - then this is not used as it will overfly badly if (g.waypoint_max_radius > 0 && auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) { - if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) { + if (current_loc.past_interval_finish_line(prev_WP_loc, flex_next_WP_loc)) { // this is needed to ensure completion of the waypoint if (cmd_passby == 0) { prev_WP_loc = current_loc; @@ -644,7 +644,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } // have we flown past the waypoint? - if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) { + if (current_loc.past_interval_finish_line(prev_WP_loc, flex_next_WP_loc)) { gcs().send_text(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um", (unsigned)mission.get_current_nav_cmd().index, (unsigned)current_loc.get_distance(flex_next_WP_loc)); @@ -1038,7 +1038,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd) // check if we should move on to the next waypoint Location breakout_loc = cmd.content.location; breakout_loc.offset_bearing(vtol_approach_s.approach_direction_deg + 180, quadplane.stopping_distance()); - if(location_passed_point(current_loc, start, breakout_loc)) { + if(current_loc.past_interval_finish_line(start, breakout_loc)) { vtol_approach_s.approach_stage = VTOL_LANDING; quadplane.do_vtol_land(cmd); // fallthrough diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 0535d36998..a1366dadb6 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -87,8 +87,7 @@ void Plane::navigate() // waypoint distance from plane // ---------------------------- auto_state.wp_distance = current_loc.get_distance(next_WP_loc); - auto_state.wp_proportion = location_path_proportion(current_loc, - prev_WP_loc, next_WP_loc); + auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc); SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion); // update total loiter angle diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e7880f7cd4..de82c2df14 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2169,8 +2169,7 @@ void QuadPlane::vtol_position_controller(void) float target_altitude = plane.next_WP_loc.alt; if (poscontrol.slow_descent) { // gradually descend as we approach target - plane.auto_state.wp_proportion = location_path_proportion(plane.current_loc, - plane.prev_WP_loc, plane.next_WP_loc); + plane.auto_state.wp_proportion = plane.current_loc.line_path_proportion(plane.prev_WP_loc, plane.next_WP_loc); target_altitude = linear_interpolate(plane.prev_WP_loc.alt, plane.next_WP_loc.alt, plane.auto_state.wp_proportion,