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
// 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;
}

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) ||
(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;
}

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.
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;

35
ArduPlane/avoidance_adsb.cpp
View File

@ -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;
}

4
ArduPlane/avoidance_adsb.h
View File

@ -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;

6
ArduPlane/commands.cpp
View File

@ -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
// -----------------------------------------------

22
ArduPlane/fence.cpp
View File

@ -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) {

10
ArduPlane/mode_LoiterAltQLand.cpp
View File

@ -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;
}

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
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;
}