Plane: terrain_navigation: correct position of loiter centre

Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Feb 23, 2024 · c729424 · c729424
1 parent 46dc6c2
commit c729424
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 24 deletions.
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -343,6 +343,7 @@ class ModeTerrainNavigation : public ModeGuided
 private:
     float _radius_m;
     Vector2f _unit_path_tangent;
+    enum { NAV_LOITER=0, NAV_WAYPOINT=1 } _nav_mode{NAV_LOITER};
 };
 
 class ModeCircle: public Mode

diff --git a/ArduPlane/mode_terrain_navigation.cpp b/ArduPlane/mode_terrain_navigation.cpp
@@ -5,9 +5,6 @@
 
 bool ModeTerrainNavigation::_enter()
 {
-    gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::_enter");
-    // return ModeGuided::_enter();
-
 #if ENABLE_NPFG_CONTROLLER
     // switch to NPFG nav controller
     plane.nav_controller = &plane.NPFG_controller;
@@ -19,47 +16,52 @@ bool ModeTerrainNavigation::_enter()
 
 void ModeTerrainNavigation::_exit()
 {
-    gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::_exit");
-
 #if ENABLE_NPFG_CONTROLLER
     // restore default nav controller
     plane.nav_controller = &plane.L1_controller;
 #else
 #endif
-
 }
 
 void ModeTerrainNavigation::update()
 {
-    gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::update");
-    // ModeGuided::update();
-
     plane.calc_nav_roll();
 
     plane.calc_nav_pitch();
 
-    // Throttle output
-
-    // TECS control
     plane.calc_throttle();
 }
 
 void ModeTerrainNavigation::navigate()
 {
-    gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::navigate");
-    // ModeGuided::navigate();
-
 #if ENABLE_NPFG_CONTROLLER
     plane.NPFG_controller.set_path_tangent(_unit_path_tangent);
     plane.NPFG_controller.update_loiter(plane.next_WP_loc, _radius_m,
         plane.loiter.direction);
 #else
-    //
     if (_radius_m > 0.0) {
+        // moving along arc of circle - loiter about wp located at
+        // centre of curvature.
         auto center_wp = plane.next_WP_loc;
+        Vector3p tangent_ned(_unit_path_tangent.x, _unit_path_tangent.y, 1.0);
+        Vector3p dn_ned(0.0, 0.0, 1.0); 
+        auto ofs_ned = dn_ned.cross(tangent_ned)
+            * _radius_m * plane.loiter.direction;
+        center_wp.offset(ofs_ned);
+
         plane.nav_controller->update_loiter(center_wp, _radius_m,
             plane.loiter.direction);
+
+        // notify when we switch mode
+        if (_nav_mode == NAV_WAYPOINT) {
+          gcs().send_text(MAV_SEVERITY_INFO,
+              "NAV_LOITER: radius: %f, direction: %d", _radius_m,
+              plane.loiter.direction);
+        }
+        _nav_mode = NAV_LOITER;
     } else {
+        // moving along a line segment - navigate to wp ahead of closest point
+        // in direction of path tangent 
         float ofs_m = 50.0;
         Vector3p ofs_ned(ofs_m * _unit_path_tangent.x,
             ofs_m * _unit_path_tangent.y, 0.0);
@@ -68,24 +70,25 @@ void ModeTerrainNavigation::navigate()
         next_wp.offset(ofs_ned);
 
         plane.nav_controller->update_waypoint(prev_wp, next_wp);
+
+        // notify when we switch mode
+        if (_nav_mode == NAV_LOITER) {
+          gcs().send_text(MAV_SEVERITY_INFO, "NAV_WAYPOINT: offset: %f", ofs_m);
+        }
+        _nav_mode = NAV_WAYPOINT;
     }
 
 #endif
 }
 
 bool ModeTerrainNavigation::handle_guided_request(Location target_loc)
 {
-    gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::handle_guided_request");
-    // return ModeGuided::handle_guided_request(target_loc);
-
     // add home alt if needed
     if (target_loc.relative_alt) {
         target_loc.alt += plane.home.alt;
         target_loc.relative_alt = 0;
     }
 
-    // see: Plane::set_guided_WP
-
     // copy the current location into the OldWP slot
     plane.prev_WP_loc = plane.current_loc;
 
@@ -97,9 +100,6 @@ bool ModeTerrainNavigation::handle_guided_request(Location target_loc)
 
 void ModeTerrainNavigation::set_radius_and_direction(const float radius, const bool direction_is_ccw)
 {
-    // gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::set_radius_and_direction");
-    // ModeGuided::set_radius_and_direction(radius, direction_is_ccw);
-
     _radius_m = radius;
     plane.loiter.direction = direction_is_ccw ? -1 : 1;
 }