AP_TECS: add ground slope estimate for landing on sloped surfaces

jschall · Jul 8, 2024 · 9dafe24 · 9dafe24
1 parent f6a095e
commit 9dafe24
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 4 deletions.
diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
@@ -561,8 +561,19 @@ void AP_TECS::_update_height_demand(void)
         _sink_rate_limit = _maxSinkRate_approach;
     }
 
+    const float groundspeed = _ahrs.groundspeed();
+    if (groundspeed > 0.5) {
+        const float max_ground_slope = 0.15;
+        const float coef = MIN(_DT / (_DT + MAX(0.5f, _DT)), 1.0f);
+        const float ground_height = _height-_hgt_afe;
+        const float ground_slope = (ground_height-_last_ground_height)/(groundspeed*_DT);
+        _ground_slope_filtered += (ground_slope-_ground_slope_filtered)*coef;
+        _ground_slope_filtered = constrain_float(_ground_slope_filtered, -max_ground_slope, max_ground_slope);
+        _last_ground_height = ground_height;
+    }
+
     if (_flags.is_doing_auto_land) {
-        float land_sink_rate_adj = _land_sink + _land_sink_rate_change*_distance_beyond_land_wp;
+        float land_sink_rate_adj = _land_sink + _land_sink_rate_change*_distance_beyond_land_wp - _ground_slope_filtered*groundspeed;
         float land_sink_rate_limit = MAX(land_sink_rate_adj + (_hgt_afe-_flare_holdoff_hgt)/timeConstant(),land_sink_rate_adj);
         _sink_rate_limit = MIN(land_sink_rate_limit, _sink_rate_limit);
     }
@@ -578,9 +589,8 @@ void AP_TECS::_update_height_demand(void)
 
         _hgt_dem = _hgt_dem_in_raw;
     } else {
-//         _hgt_rate_dem += (-_sink_rate_limit - _hgt_rate_dem)*(_DT/(_DT*timeConstant()));
-        _hgt_rate_dem = constrain_float(_hgt_rate_dem, -_sink_rate_limit, -_land_sink);
-//         _hgt_rate_dem = -_sink_rate_limit;
+        _hgt_rate_dem = -_sink_rate_limit;
+//         _hgt_rate_dem = constrain_float(_hgt_rate_dem, -_sink_rate_limit, _climb_rate_limit);
         _hgt_dem = _height;
     }
 }

diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
@@ -309,6 +309,8 @@ class AP_TECS {
     float _hgt_dem_lpf;         // height demand after application of low pass filtering (m)
     float _hgt_dem;             // height demand sent to control loops (m)
     float _hgt_dem_prev;        // _hgt_dem from previous frame (m)
+    float _last_ground_height;
+    float _ground_slope_filtered;
 
     // height rate demands
     float _hgt_rate_dem;        // height rate demand sent to control loops