Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Specify starting location to set_offset_altitude_location

Browse Source
This commit is contained in:
Samuel Tabor 2021-11-10 18:42:59 +00:00 committed by Peter Barker
parent cceae1acf3
commit a715472353
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/Plane.h
View File

@ -843,7 +843,7 @@ private:
int32_t calc_altitude_error_cm(void); int32_t calc_altitude_error_cm(void);
void check_fbwb_minimum_altitude(void); void check_fbwb_minimum_altitude(void);
void reset_offset_altitude(void); void reset_offset_altitude(void);
void set_offset_altitude_location(const Location &loc); void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
bool above_location_current(const Location &loc); bool above_location_current(const Location &loc);
void setup_terrain_target_alt(Location &loc) const; void setup_terrain_target_alt(Location &loc) const;
int32_t adjusted_altitude_cm(void); int32_t adjusted_altitude_cm(void);

16
ArduPlane/altitude.cpp
View File

@ -112,7 +112,7 @@ void Plane::setup_glide_slope(void)
https://github.com/ArduPilot/ardupilot/issues/39 https://github.com/ArduPilot/ardupilot/issues/39
*/ */
if (above_location_current(next_WP_loc)) { if (above_location_current(next_WP_loc)) {
set_offset_altitude_location(next_WP_loc); set_offset_altitude_location(prev_WP_loc, next_WP_loc);
} else { } else {
reset_offset_altitude(); reset_offset_altitude();
} }
@ -125,7 +125,7 @@ void Plane::setup_glide_slope(void)
// gain height at low altitudes, potentially hitting // gain height at low altitudes, potentially hitting
// obstacles. // obstacles.
if (adjusted_relative_altitude_cm() > 2000 || above_location_current(next_WP_loc)) { if (adjusted_relative_altitude_cm() > 2000 || above_location_current(next_WP_loc)) {
set_offset_altitude_location(next_WP_loc); set_offset_altitude_location(prev_WP_loc, next_WP_loc);
} else { } else {
reset_offset_altitude(); reset_offset_altitude();
} }
@ -320,7 +320,7 @@ void Plane::set_target_altitude_proportion(const Location &loc, float proportion
if(g.glide_slope_threshold > 0) { if(g.glide_slope_threshold > 0) {
if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) { if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) {
set_target_altitude_location(loc); set_target_altitude_location(loc);
set_offset_altitude_location(loc); set_offset_altitude_location(current_loc, loc);
change_target_altitude(-target_altitude.offset_cm*proportion); change_target_altitude(-target_altitude.offset_cm*proportion);
//adjust the new target offset altitude to reflect that we are partially already done //adjust the new target offset altitude to reflect that we are partially already done
if(proportion > 0.0f) if(proportion > 0.0f)
@ -392,13 +392,13 @@ void Plane::reset_offset_altitude(void)
/* /*
reset the altitude offset used for glide slopes, based on difference reset the altitude offset used for glide slopes, based on difference
between altitude at a destination and current altitude. If between altitude at a destination and a specified start altitude. If
destination is above the current altitude then the result is destination is above the starting altitude then the result is
positive. positive.
*/ */
void Plane::set_offset_altitude_location(const Location &loc) void Plane::set_offset_altitude_location(const Location &start_loc, const Location &destination_loc)
{ {
target_altitude.offset_cm = loc.alt - current_loc.alt; target_altitude.offset_cm = destination_loc.alt - start_loc.alt;
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
/* /*
@ -407,7 +407,7 @@ void Plane::set_offset_altitude_location(const Location &loc)
terrain altitude terrain altitude
*/ */
float height; float height;
if (loc.terrain_alt && if (destination_loc.terrain_alt &&
target_altitude.terrain_following && target_altitude.terrain_following &&
terrain.height_above_terrain(height, true)) { terrain.height_above_terrain(height, true)) {
target_altitude.offset_cm = target_altitude.terrain_alt_cm - (height * 100); target_altitude.offset_cm = target_altitude.terrain_alt_cm - (height * 100);