diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index c7de58ee5a..34df07b2bc 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -1500,4 +1500,31 @@ void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) { // don't allow max pitch to go below min pitch _PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf); +} + +void AP_TECS::offset_altitude(const float alt_offset) +{ + // Convention: When alt_offset is positive it means that the altitude of + // home has increased. Thus, the relative altitude of the vehicle has + // decreased. + // + // Assumption: This method is called more often and before + // `update_pitch_throttle()`. This is necessary to ensure that new height + // demands which incorporate the home change are compatible with the + // (now updated) internal height state. + + _flare_hgt_dem_ideal -= alt_offset; + _flare_hgt_dem_adj -= alt_offset; + _hgt_at_start_of_flare -= alt_offset; + _hgt_dem_in_prev -= alt_offset; + _hgt_dem_lpf -= alt_offset; + _hgt_dem_rate_ltd -= alt_offset; + _hgt_dem_prev -= alt_offset; + _height_filter.height -= alt_offset; + + // The following variables are updated anew in every call of + // `update_pitch_throttle()`. There's no need to update those. + // _hgt_dem + // _hgt_dem_in_raw + // _hgt_dem_in } \ No newline at end of file diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 57748edba1..4c4343b5ee 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -154,6 +154,9 @@ public: _need_reset = true; } + // Apply an altitude offset, to compensate for changes in home alt. + void offset_altitude(const float alt_offset); + // this supports the TECS_* user settable parameters static const struct AP_Param::GroupInfo var_info[];