From 64fe9261ded09ea6a0263f23bd9719b17faf08db Mon Sep 17 00:00:00 2001
From: Rhys Mainwaring <rhys.mainwaring@me.com>
Date: Mon, 26 Feb 2024 12:36:54 +0000
Subject: [PATCH] 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 <rhys.mainwaring@me.com>
---
 ArduPlane/mode.h                      |  2 +-
 ArduPlane/mode_terrain_navigation.cpp | 22 +++++++++-------------
 2 files changed, 10 insertions(+), 14 deletions(-)

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 6dbe102de32577..17edd6e4526969 100644
--- 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
index 320ea2cd3d06cc..88085c840ade79 100644
--- 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) {