Plane: terrain_navigation: use ModeGuided update

- Call ModeGuided _enter and _exit methods. - Use ModeGuided update method. - Store commanded curvature rather than radius. Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Feb 26, 2024 · 64fe926 · 64fe926
1 parent 6ef47e5
commit 64fe926
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 14 deletions.
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -341,7 +341,7 @@ class ModeTerrainNavigation : public ModeGuided
     void _exit() override;
 
 private:
-    float _radius_m;
+    float _curvature;
     Vector2f _unit_path_tangent;
 };
 

diff --git a/ArduPlane/mode_terrain_navigation.cpp b/ArduPlane/mode_terrain_navigation.cpp
@@ -9,7 +9,7 @@ bool ModeTerrainNavigation::_enter()
     // switch to NPFG nav controller
     plane.nav_controller = &plane.NPFG_controller;
 #endif
-    return true;
+    return ModeGuided::_enter();
 }
 
 void ModeTerrainNavigation::_exit()
@@ -18,25 +18,18 @@ void ModeTerrainNavigation::_exit()
     // restore default nav controller
     plane.nav_controller = &plane.L1_controller;
 #endif
+    ModeGuided::_exit();
 }
 
 void ModeTerrainNavigation::update()
 {
-    plane.calc_nav_roll();
-
-    plane.calc_nav_pitch();
-
-    plane.calc_throttle();
+    ModeGuided::update();
 }
 
 void ModeTerrainNavigation::navigate()
 {
-    float curvature = 0.0;
-    if (!is_zero(_radius_m)) {
-        curvature = 1.0 / _radius_m;
-    }
     plane.nav_controller->update_path(plane.next_WP_loc, _unit_path_tangent,
-        curvature, plane.loiter.direction);
+        _curvature, plane.loiter.direction);
 }
 
 bool ModeTerrainNavigation::handle_guided_request(Location target_loc)
@@ -58,8 +51,11 @@ bool ModeTerrainNavigation::handle_guided_request(Location target_loc)
 
 void ModeTerrainNavigation::set_radius_and_direction(const float radius, const bool direction_is_ccw)
 {
-    _radius_m = radius;
-    plane.loiter.direction = direction_is_ccw ? -1 : 1;
+    _curvature = 0.0;
+    if (!is_zero(radius)) {
+        _curvature = 1.0 / radius;
+    }
+    ModeGuided::set_radius_and_direction(radius, direction_is_ccw);
 }
 
 void ModeTerrainNavigation::set_path_tangent(Vector2f unit_path_tangent) {