AP_Terrain: added terrain reference adjustment

this restores the terrain adjustment functionality removed in #19946,
but without the problematic approach of always using home (which can
be moved in flight) and with a TERR_OFS_MAX parameter to limit the
amount of adjustment
This commit is contained in:
Andrew Tridgell 2022-03-28 08:26:49 +11:00
parent 4bc35ba04d
commit 89465d2126
3 changed files with 112 additions and 2 deletions

View File

@ -68,6 +68,14 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
// @User: Advanced
AP_GROUPINFO("MARGIN", 3, AP_Terrain, margin, 0.05),
// @Param: OFS_MAX
// @DisplayName: Maximum reference offset
// @Description: The maximum adjustment of terrain altitude based on the assumption that the vehicle is on the ground when it is armed. When the vehicle is armed the location of the vehicle is recorded, and when terrain data is available for that location a height adjustment for terrain data is calculated that aligns the terrain height at that location with the altitude recorded at arming. This height adjustment is applied to all terrain data. This parameter clamps the amount of adjustment. A value of zero disables the use of terrain height adjustment.
// @Units: m
// @Range: 0 50
// @User: Advanced
AP_GROUPINFO("OFS_MAX", 4, AP_Terrain, offset_max, 15),
AP_GROUPEND
};
@ -95,7 +103,7 @@ AP_Terrain::AP_Terrain() :
This function costs about 20 microseconds on Pixhawk
*/
bool AP_Terrain::height_amsl(const Location &loc, float &height)
bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
{
if (!allocate()) {
return false;
@ -107,6 +115,9 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
if (loc.lat == home_loc.lat &&
loc.lng == home_loc.lng) {
height = home_height;
if (corrected && have_reference_offset) {
height += reference_offset;
}
return true;
}
@ -157,6 +168,10 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
home_loc = loc;
}
if (corrected && have_reference_offset) {
height += reference_offset;
}
return true;
}
@ -404,6 +419,78 @@ bool AP_Terrain::allocate(void)
return true;
}
/*
setup a reference location for terrain adjustment. This should
be called when the vehicle is definately on the ground
*/
void AP_Terrain::set_reference_location(void)
{
const auto &ahrs = AP::ahrs();
// check we have absolute position
nav_filter_status status;
if (!ahrs.get_filter_status(status) ||
!status.flags.vert_pos ||
!status.flags.horiz_pos_abs ||
!status.flags.attitude) {
return;
}
// check we have a small 3D velocity
Vector3f vel;
if (!ahrs.get_velocity_NED(vel) ||
vel.length() > 3) {
return;
}
have_reference_offset = false;
have_reference_loc = ahrs.get_location(reference_loc);
update_reference_offset();
}
/*
get the offset between terrain height and reference alt at the
reference location
*/
void AP_Terrain::update_reference_offset(void)
{
// TERR_OFS_MAX of zero means no adjustment
if (!is_positive(offset_max)) {
have_reference_offset = false;
return;
}
// allow for change to TERRAIN_OFS_MAX while flying
if (have_reference_offset) {
reference_offset = constrain_float(reference_offset, -offset_max, offset_max);
return;
}
if (!have_reference_loc) {
// no reference available yet
return;
}
// calculate adjustment
float height;
if (!height_amsl(reference_loc, height)) {
return;
}
int32_t alt_cm;
if (!reference_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
return;
}
float adjustment = alt_cm*0.01 - height;
reference_offset = constrain_float(adjustment, -offset_max, offset_max);
if (fabsf(adjustment) > offset_max.get()+0.5) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Terrain: clamping offset %.0f to %.0f",
adjustment, reference_offset);
}
have_reference_offset = true;
}
namespace AP {
AP_Terrain *terrain()

View File

@ -122,7 +122,7 @@ public:
return false if not available
*/
bool height_amsl(const Location &loc, float &height);
bool height_amsl(const Location &loc, float &height, bool corrected = true);
/*
find difference between home terrain height and the terrain
@ -189,6 +189,12 @@ public:
*/
bool init_failed() const { return memory_alloc_failed; }
/*
setup a reference location for terrain adjustment. This should
be called when the vehicle is definately on the ground
*/
void set_reference_location(void);
private:
// allocate the terrain subsystem data
bool allocate(void);
@ -344,12 +350,18 @@ private:
*/
void update_rally_data(void);
/*
calculate reference offset if needed
*/
void update_reference_offset(void);
// parameters
AP_Int8 enable;
AP_Float margin;
AP_Int16 grid_spacing; // meters between grid points
AP_Int16 options; // option bits
AP_Float offset_max;
enum class Options {
DisableDownload = (1U<<0),
@ -396,6 +408,15 @@ private:
float home_height;
Location home_loc;
// reference position for terrain adjustment, set at arming
bool have_reference_loc;
Location reference_loc;
// calculated reference offset
bool have_reference_offset;
float reference_offset;
// cache the last terrain height (AMSL) of the AHRS current
// location. This is used for extrapolation when terrain data is
// temporarily unavailable

View File

@ -351,6 +351,8 @@ void AP_Terrain::io_timer(void)
return;
}
update_reference_offset();
switch (disk_io_state) {
case DiskIoIdle:
case DiskIoDoneRead: