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

Plane: remove persistent guided_WP_loc state 

So instead of updating plane.guided_WP_loc and then calling
set_guided_WP(void) to copy that state into plane.next_WP_loc we pass
the new location in the call to set_guided_WP(const Location &loc).

avoidance was the only place which was not entirely over-writing
plane.guided_WP_loc.  However, plane.next_WP_loc was updated to be the
current location when we entered guided mode.  If we update the
horizontal/vertical avoidance now it is relative to the current
location, not the guided wp location, which could be quite important.
This commit is contained in:
Peter Barker 2022-02-10 10:44:58 +11:00 committed by Andrew Tridgell
parent feb9e78f00
commit 2784f8fa7f
9 changed files with 59 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/ArduPlane.cpp
View File

@ -713,19 +713,20 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
#if AP_SCRIPTING_ENABLED #if AP_SCRIPTING_ENABLED
// set target location (for use by scripting) // 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) { if (plane.control_mode != &plane.mode_guided) {
// only accept position updates when in GUIDED mode // only accept position updates when in GUIDED mode
return false; return false;
} }
plane.guided_WP_loc = target_loc;
// add home alt if needed // add home alt if needed
if (plane.guided_WP_loc.relative_alt) { if (loc.relative_alt) {
plane.guided_WP_loc.alt += plane.home.alt; loc.alt += plane.home.alt;
plane.guided_WP_loc.relative_alt = 0; loc.relative_alt = 0;
} }
plane.set_guided_WP(); plane.set_guided_WP(loc);
return true; return true;
} }

9
ArduPlane/GCS_Mavlink.cpp
View File

@ -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) || if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == &plane.mode_guided)) { (plane.control_mode == &plane.mode_guided)) {
plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
plane.guided_WP_loc = requested_position;
// add home alt if needed // add home alt if needed
if (plane.guided_WP_loc.relative_alt) { if (requested_position.relative_alt) {
plane.guided_WP_loc.alt += plane.home.alt; requested_position.alt += plane.home.alt;
plane.guided_WP_loc.relative_alt = 0; requested_position.relative_alt = 0;
} }
plane.set_guided_WP(); plane.set_guided_WP(requested_position);
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }

5
ArduPlane/Plane.h
View File

@ -721,9 +721,6 @@ private:
// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. // The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
Location next_WP_loc {}; Location next_WP_loc {};
// The location of the active waypoint in Guided mode.
struct Location guided_WP_loc {};
// Altitude control // Altitude control
struct { struct {
// target altitude above sea level in cm. Used for barometric // target altitude above sea level in cm. Used for barometric
@ -947,7 +944,7 @@ private:
#endif #endif
// commands.cpp // commands.cpp
void set_guided_WP(void); void set_guided_WP(const Location &loc);
void update_home(); void update_home();
// set home location and store it persistently: // set home location and store it persistently:
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED; bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;

35
ArduPlane/avoidance_adsb.cpp
View File

@ -61,31 +61,34 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
} }
break; break;
case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: {
// climb or descend to avoid obstacle // climb or descend to avoid obstacle
if (handle_avoidance_vertical(obstacle, failsafe_state_change)) { Location loc = plane.next_WP_loc;
plane.set_guided_WP(); if (handle_avoidance_vertical(obstacle, failsafe_state_change, loc)) {
plane.set_guided_WP(loc);
} else { } else {
actual_action = MAV_COLLISION_ACTION_NONE; actual_action = MAV_COLLISION_ACTION_NONE;
} }
break; break;
}
case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: {
// move horizontally to avoid obstacle // move horizontally to avoid obstacle
if (handle_avoidance_horizontal(obstacle, failsafe_state_change)) { Location loc = plane.next_WP_loc;
plane.set_guided_WP(); if (handle_avoidance_horizontal(obstacle, failsafe_state_change, loc)) {
plane.set_guided_WP(loc);
} else { } else {
actual_action = MAV_COLLISION_ACTION_NONE; actual_action = MAV_COLLISION_ACTION_NONE;
} }
break; break;
}
case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR: case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR:
{ {
// move horizontally and vertically to avoid obstacle // move horizontally and vertically to avoid obstacle
const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change); Location loc = plane.next_WP_loc;
const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change); 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) { if (success_vert || success_hor) {
plane.set_guided_WP(); plane.set_guided_WP(loc);
} else { } else {
actual_action = MAV_COLLISION_ACTION_NONE; 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); 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 // ensure copter is in avoid_adsb mode
if (!check_flightmode(allow_mode_change)) { 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 // get best vector away from obstacle
if (plane.current_loc.alt > obstacle->_location.alt) { if (plane.current_loc.alt > obstacle->_location.alt) {
// should climb // 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; return true;
} else if (plane.current_loc.alt > plane.g.RTL_altitude_cm) { } else if (plane.current_loc.alt > plane.g.RTL_altitude_cm) {
// should descend while above RTL alt // should descend while above RTL alt
// TODO: consider using a lower altitude than RTL_altitude_cm since it's default (100m) is quite high // 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 true;
} }
return false; 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 // ensure plane is in avoid_adsb mode
if (!check_flightmode(allow_mode_change)) { if (!check_flightmode(allow_mode_change)) {
@ -205,7 +208,7 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl
velocity_neu *= 10000; velocity_neu *= 10000;
// set target // set target
plane.guided_WP_loc.offset(velocity_neu.x, velocity_neu.y); new_loc.offset(velocity_neu.x, velocity_neu.y);
return true; return true;
} }

4
ArduPlane/avoidance_adsb.h
View File

@ -26,10 +26,10 @@ protected:
bool check_flightmode(bool allow_mode_change); bool check_flightmode(bool allow_mode_change);
// vertical avoidance handler // 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 // 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 // control mode before avoidance began
enum Mode::Number prev_control_mode_number = Mode::Number::RTL; enum Mode::Number prev_control_mode_number = Mode::Number::RTL;

6
ArduPlane/commands.cpp
View File

@ -61,9 +61,9 @@ void Plane::set_next_WP(const struct Location &loc)
setup_turn_angle(); 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; loiter.direction = -1;
} else { } else {
loiter.direction = 1; loiter.direction = 1;
@ -75,7 +75,7 @@ void Plane::set_guided_WP(void)
// Load the next_WP slot // 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 // used to control FBW and limit the rate of climb
// ----------------------------------------------- // -----------------------------------------------

22
ArduPlane/fence.cpp
View File

@ -70,39 +70,39 @@ void Plane::fence_check()
set_mode(mode_guided, ModeReason::FENCE_BREACHED); set_mode(mode_guided, ModeReason::FENCE_BREACHED);
} }
Location loc;
if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { 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 { } else {
//return to fence return point, not a rally point //return to fence return point, not a rally point
guided_WP_loc = {};
if (fence.get_return_altitude() > 0) { if (fence.get_return_altitude() > 0) {
// fly to the return point using _retalt // 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()) { } else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) {
// invalid min/max, use RTL_altitude // 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 { } else {
// fly to the return point, with an altitude half way between // fly to the return point, with an altitude half way between
// min and max // 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; Vector2l return_point;
if(fence.polyfence().get_return_point(return_point)) { if(fence.polyfence().get_return_point(return_point)) {
guided_WP_loc.lat = return_point[0]; loc.lat = return_point[0];
guided_WP_loc.lng = return_point[1]; loc.lng = return_point[1];
} else { } else {
// When no fence return point is found (ie. no inclusion fence uploaded, but exclusion is) // 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 // we fail to obtain a valid fence return point. In this case, home is considered a safe
// return point. // return point.
guided_WP_loc.lat = home.lat; loc.lat = home.lat;
guided_WP_loc.lng = home.lng; loc.lng = home.lng;
} }
} }
if (fence.get_action() != AC_FENCE_ACTION_RTL_AND_LAND) { if (fence.get_action() != AC_FENCE_ACTION_RTL_AND_LAND) {
setup_terrain_target_alt(guided_WP_loc); setup_terrain_target_alt(loc);
set_guided_WP(); set_guided_WP(loc);
} }
if (fence.get_action() == AC_FENCE_ACTION_GUIDED_THROTTLE_PASS) { if (fence.get_action() == AC_FENCE_ACTION_GUIDED_THROTTLE_PASS) {

10
ArduPlane/mode_LoiterAltQLand.cpp
View File

@ -44,20 +44,18 @@ void ModeLoiterAltQLand::switch_qland()
bool ModeLoiterAltQLand::handle_guided_request(Location target_loc) bool ModeLoiterAltQLand::handle_guided_request(Location target_loc)
{ {
plane.guided_WP_loc = target_loc;
// setup altitude // setup altitude
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { 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 { } 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 #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 #endif
plane.set_guided_WP(); plane.set_guided_WP(target_loc);
return true; return true;
} }

18
ArduPlane/mode_guided.cpp
View File

@ -8,19 +8,19 @@ bool ModeGuided::_enter()
when entering guided mode we set the target as the current when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code 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 HAL_QUADPLANE_ENABLED
if (plane.quadplane.guided_mode_enabled()) { if (plane.quadplane.guided_mode_enabled()) {
/* /*
if using Q_GUIDED_MODE then project forward by the stopping distance if using Q_GUIDED_MODE then project forward by the stopping distance
*/ */
plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()),
plane.quadplane.stopping_distance()); plane.quadplane.stopping_distance());
} }
#endif #endif
plane.set_guided_WP(); plane.set_guided_WP(loc);
return true; return true;
} }
@ -45,15 +45,13 @@ void ModeGuided::navigate()
bool ModeGuided::handle_guided_request(Location target_loc) bool ModeGuided::handle_guided_request(Location target_loc)
{ {
plane.guided_WP_loc = target_loc;
// add home alt if needed // add home alt if needed
if (plane.guided_WP_loc.relative_alt) { if (target_loc.relative_alt) {
plane.guided_WP_loc.alt += plane.home.alt; target_loc.alt += plane.home.alt;
plane.guided_WP_loc.relative_alt = 0; target_loc.relative_alt = 0;
} }
plane.set_guided_WP(); plane.set_guided_WP(target_loc);
return true; return true;
} }