diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 7d67005f27..5910ec9636 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -713,19 +713,20 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const #if AP_SCRIPTING_ENABLED // set target location (for use by scripting) -bool Plane::set_target_location(const Location& target_loc) +bool Plane::set_target_location(const Location &target_loc) { + Location loc{target_loc}; + if (plane.control_mode != &plane.mode_guided) { // only accept position updates when in GUIDED mode return false; } - plane.guided_WP_loc = target_loc; // add home alt if needed - if (plane.guided_WP_loc.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.relative_alt = 0; + if (loc.relative_alt) { + loc.alt += plane.home.alt; + loc.relative_alt = 0; } - plane.set_guided_WP(); + plane.set_guided_WP(loc); return true; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 884b1cb0b0..07a23adeb3 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -714,15 +714,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); - plane.guided_WP_loc = requested_position; // add home alt if needed - if (plane.guided_WP_loc.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.relative_alt = 0; + if (requested_position.relative_alt) { + requested_position.alt += plane.home.alt; + requested_position.relative_alt = 0; } - plane.set_guided_WP(); + plane.set_guided_WP(requested_position); return MAV_RESULT_ACCEPTED; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index bb82150de6..05905a4f46 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -721,9 +721,6 @@ private: // The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. Location next_WP_loc {}; - // The location of the active waypoint in Guided mode. - struct Location guided_WP_loc {}; - // Altitude control struct { // target altitude above sea level in cm. Used for barometric @@ -947,7 +944,7 @@ private: #endif // commands.cpp - void set_guided_WP(void); + void set_guided_WP(const Location &loc); void update_home(); // set home location and store it persistently: bool set_home_persistently(const Location &loc) WARN_IF_UNUSED; diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index b44ff75b96..f775fa29c8 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -61,31 +61,34 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob } break; - case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: + case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: { // climb or descend to avoid obstacle - if (handle_avoidance_vertical(obstacle, failsafe_state_change)) { - plane.set_guided_WP(); + Location loc = plane.next_WP_loc; + if (handle_avoidance_vertical(obstacle, failsafe_state_change, loc)) { + plane.set_guided_WP(loc); } else { actual_action = MAV_COLLISION_ACTION_NONE; } break; - - case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: + } + case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: { // move horizontally to avoid obstacle - if (handle_avoidance_horizontal(obstacle, failsafe_state_change)) { - plane.set_guided_WP(); + Location loc = plane.next_WP_loc; + if (handle_avoidance_horizontal(obstacle, failsafe_state_change, loc)) { + plane.set_guided_WP(loc); } else { actual_action = MAV_COLLISION_ACTION_NONE; } break; - + } case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR: { // move horizontally and vertically to avoid obstacle - const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change); - const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change); + Location loc = plane.next_WP_loc; + const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change, loc); + const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change, loc); if (success_vert || success_hor) { - plane.set_guided_WP(); + plane.set_guided_WP(loc); } else { actual_action = MAV_COLLISION_ACTION_NONE; } @@ -157,7 +160,7 @@ bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change) return (plane.control_mode == &plane.mode_avoidADSB); } -bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change) +bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc) { // ensure copter is in avoid_adsb mode if (!check_flightmode(allow_mode_change)) { @@ -167,20 +170,20 @@ bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle // get best vector away from obstacle if (plane.current_loc.alt > obstacle->_location.alt) { // should climb - plane.guided_WP_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX + new_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX return true; } else if (plane.current_loc.alt > plane.g.RTL_altitude_cm) { // should descend while above RTL alt // TODO: consider using a lower altitude than RTL_altitude_cm since it's default (100m) is quite high - plane.guided_WP_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX + new_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX return true; } return false; } -bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change) +bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc) { // ensure plane is in avoid_adsb mode if (!check_flightmode(allow_mode_change)) { @@ -205,7 +208,7 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl velocity_neu *= 10000; // set target - plane.guided_WP_loc.offset(velocity_neu.x, velocity_neu.y); + new_loc.offset(velocity_neu.x, velocity_neu.y); return true; } diff --git a/ArduPlane/avoidance_adsb.h b/ArduPlane/avoidance_adsb.h index 23cf57ba19..508fe65877 100644 --- a/ArduPlane/avoidance_adsb.h +++ b/ArduPlane/avoidance_adsb.h @@ -26,10 +26,10 @@ protected: bool check_flightmode(bool allow_mode_change); // vertical avoidance handler - bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change); + bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc); // horizontal avoidance handler - bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change); + bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc); // control mode before avoidance began enum Mode::Number prev_control_mode_number = Mode::Number::RTL; diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index c5bcee335b..3a0900acdb 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -61,9 +61,9 @@ void Plane::set_next_WP(const struct Location &loc) setup_turn_angle(); } -void Plane::set_guided_WP(void) +void Plane::set_guided_WP(const Location &loc) { - if (aparm.loiter_radius < 0 || guided_WP_loc.loiter_ccw) { + if (aparm.loiter_radius < 0 || loc.loiter_ccw) { loiter.direction = -1; } else { loiter.direction = 1; @@ -75,7 +75,7 @@ void Plane::set_guided_WP(void) // Load the next_WP slot // --------------------- - next_WP_loc = guided_WP_loc; + next_WP_loc = loc; // used to control FBW and limit the rate of climb // ----------------------------------------------- diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index d22a50bb75..ee829cabb9 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -70,39 +70,39 @@ void Plane::fence_check() set_mode(mode_guided, ModeReason::FENCE_BREACHED); } + Location loc; if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { - guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); + loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); } else { //return to fence return point, not a rally point - guided_WP_loc = {}; if (fence.get_return_altitude() > 0) { // fly to the return point using _retalt - guided_WP_loc.alt = home.alt + 100.0f * fence.get_return_altitude(); + loc.alt = home.alt + 100.0f * fence.get_return_altitude(); } else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) { // invalid min/max, use RTL_altitude - guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; + loc.alt = home.alt + g.RTL_altitude_cm; } else { // fly to the return point, with an altitude half way between // min and max - guided_WP_loc.alt = home.alt + 100.0f * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2; + loc.alt = home.alt + 100.0f * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2; } Vector2l return_point; if(fence.polyfence().get_return_point(return_point)) { - guided_WP_loc.lat = return_point[0]; - guided_WP_loc.lng = return_point[1]; + loc.lat = return_point[0]; + loc.lng = return_point[1]; } else { // When no fence return point is found (ie. no inclusion fence uploaded, but exclusion is) // we fail to obtain a valid fence return point. In this case, home is considered a safe // return point. - guided_WP_loc.lat = home.lat; - guided_WP_loc.lng = home.lng; + loc.lat = home.lat; + loc.lng = home.lng; } } if (fence.get_action() != AC_FENCE_ACTION_RTL_AND_LAND) { - setup_terrain_target_alt(guided_WP_loc); - set_guided_WP(); + setup_terrain_target_alt(loc); + set_guided_WP(loc); } if (fence.get_action() == AC_FENCE_ACTION_GUIDED_THROTTLE_PASS) { diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 586820e384..0cc8b9457c 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -44,20 +44,18 @@ void ModeLoiterAltQLand::switch_qland() bool ModeLoiterAltQLand::handle_guided_request(Location target_loc) { - plane.guided_WP_loc = target_loc; - // setup altitude #if AP_TERRAIN_AVAILABLE if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { - plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); + target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); } else { - plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); + target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); } #else - plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); + target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); #endif - plane.set_guided_WP(); + plane.set_guided_WP(target_loc); return true; } diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index aeac481989..b80e72dd02 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -8,19 +8,19 @@ bool ModeGuided::_enter() when entering guided mode we set the target as the current location. This matches the behaviour of the copter code */ - plane.guided_WP_loc = plane.current_loc; + Location loc{plane.current_loc}; #if HAL_QUADPLANE_ENABLED if (plane.quadplane.guided_mode_enabled()) { /* if using Q_GUIDED_MODE then project forward by the stopping distance */ - plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), - plane.quadplane.stopping_distance()); + loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), + plane.quadplane.stopping_distance()); } #endif - plane.set_guided_WP(); + plane.set_guided_WP(loc); return true; } @@ -45,15 +45,13 @@ void ModeGuided::navigate() bool ModeGuided::handle_guided_request(Location target_loc) { - plane.guided_WP_loc = target_loc; - // add home alt if needed - if (plane.guided_WP_loc.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.relative_alt = 0; + if (target_loc.relative_alt) { + target_loc.alt += plane.home.alt; + target_loc.relative_alt = 0; } - plane.set_guided_WP(); + plane.set_guided_WP(target_loc); return true; }