Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: use past_interval_finish_line and line_path_proportion from Location

This commit is contained in:
Pierre Kancir 2019-04-12 10:23:04 +02:00 committed by Andrew Tridgell
parent f91ffffca7
commit 65b4ba0539
6 changed files with 10 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -469,7 +469,7 @@ void Plane::update_navigation()
case Mode::Number::RTL: case Mode::Number::RTL:
if (quadplane.available() && quadplane.rtl_mode == 1 && if (quadplane.available() && quadplane.rtl_mode == 1 &&
(nav_controller->reached_loiter_target() || (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())) && auto_state.wp_distance < MAX(qrtl_radius, quadplane.stopping_distance())) &&
AP_HAL::millis() - last_mode_change_ms > 1000) { AP_HAL::millis() - last_mode_change_ms > 1000) {
/* /*
@ -588,7 +588,7 @@ void Plane::update_alt()
if (auto_throttle_mode && !throttle_suppressed) { if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0; 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); distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
} }

5
ArduPlane/altitude.cpp
View File

@ -45,7 +45,7 @@ void Plane::adjust_altitude_target()
// altitude target // altitude target
set_target_altitude_location(next_WP_loc); set_target_altitude_location(next_WP_loc);
} else if (target_altitude.offset_cm != 0 && } 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 // control climb/descent rate
set_target_altitude_proportion(next_WP_loc, 1.0f-auto_state.wp_proportion); 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, // establish the distance we are travelling to the next waypoint,
// for calculating out rate of change of altitude // for calculating out rate of change of altitude
auto_state.wp_distance = current_loc.get_distance(next_WP_loc); auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc, auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
prev_WP_loc, next_WP_loc);
SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion); SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
update_flight_stage(); update_flight_stage();

2
ArduPlane/commands.cpp
View File

@ -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 // past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately // location as the previous waypoint, to prevent immediately
// considering the waypoint complete // 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"); gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint");
prev_WP_loc = current_loc; prev_WP_loc = current_loc;
} }

6
ArduPlane/commands_logic.cpp
View File

@ -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 override with p3 - then this is not used as it will overfly badly
if (g.waypoint_max_radius > 0 && if (g.waypoint_max_radius > 0 &&
auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) { 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 // this is needed to ensure completion of the waypoint
if (cmd_passby == 0) { if (cmd_passby == 0) {
prev_WP_loc = current_loc; 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? // 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", gcs().send_text(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um",
(unsigned)mission.get_current_nav_cmd().index, (unsigned)mission.get_current_nav_cmd().index,
(unsigned)current_loc.get_distance(flex_next_WP_loc)); (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 // check if we should move on to the next waypoint
Location breakout_loc = cmd.content.location; Location breakout_loc = cmd.content.location;
breakout_loc.offset_bearing(vtol_approach_s.approach_direction_deg + 180, quadplane.stopping_distance()); 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; vtol_approach_s.approach_stage = VTOL_LANDING;
quadplane.do_vtol_land(cmd); quadplane.do_vtol_land(cmd);
// fallthrough // fallthrough

3
ArduPlane/navigation.cpp
View File

@ -87,8 +87,7 @@ void Plane::navigate()
// waypoint distance from plane // waypoint distance from plane
// ---------------------------- // ----------------------------
auto_state.wp_distance = current_loc.get_distance(next_WP_loc); auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc, auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
prev_WP_loc, next_WP_loc);
SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion); SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
// update total loiter angle // update total loiter angle

3
ArduPlane/quadplane.cpp
View File

@ -2169,8 +2169,7 @@ void QuadPlane::vtol_position_controller(void)
float target_altitude = plane.next_WP_loc.alt; float target_altitude = plane.next_WP_loc.alt;
if (poscontrol.slow_descent) { if (poscontrol.slow_descent) {
// gradually descend as we approach target // gradually descend as we approach target
plane.auto_state.wp_proportion = location_path_proportion(plane.current_loc, plane.auto_state.wp_proportion = plane.current_loc.line_path_proportion(plane.prev_WP_loc, plane.next_WP_loc);
plane.prev_WP_loc, plane.next_WP_loc);
target_altitude = linear_interpolate(plane.prev_WP_loc.alt, target_altitude = linear_interpolate(plane.prev_WP_loc.alt,
plane.next_WP_loc.alt, plane.next_WP_loc.alt,
plane.auto_state.wp_proportion, plane.auto_state.wp_proportion,