AP_L1_Control: Keep track of current nav. mode

Added navigation mode enum and member to keep track of the current mode. This makes it possible to run logic on mode changes.
ArduPilot · Jan 28, 2025 · ad924f0 · ad924f0
1 parent 75d15f1
commit ad924f0
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 0 deletions.
diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp
@@ -164,6 +164,10 @@ void AP_L1_Control::_prevent_indecision(float &Nu)
 // update L1 control for waypoint navigation
 void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &next_WP, float dist_min)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::WAYPOINT) {
+        _current_nav_mode = NavMode::WAYPOINT;
+    }
 
     Location _current_loc;
     float Nu;
@@ -308,6 +312,11 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
 // update L1 control for loitering
 void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_t loiter_direction)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::LOITER) {
+        _current_nav_mode = NavMode::LOITER;
+    }
+
     const float radius_unscaled = radius;
 
     Location _current_loc;
@@ -444,6 +453,11 @@ void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_
 // update L1 control for heading hold navigation
 void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::HEADING_HOLD) {
+        _current_nav_mode = NavMode::HEADING_HOLD;
+    }
+
     // Calculate normalised frequency for tracking loop
     const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period
     // Calculate additional damping gain
@@ -486,6 +500,11 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
 // update L1 control for level flight on current heading
 void AP_L1_Control::update_level_flight(void)
 {
+    // Update nav. mode
+    if (_current_nav_mode != NavMode::LEVEL_FLIGHT) {
+        _current_nav_mode = NavMode::LEVEL_FLIGHT;
+    }
+
     // copy to _target_bearing_cd and _nav_bearing
     _target_bearing_cd = _ahrs.yaw_sensor;
     _nav_bearing = _ahrs.get_yaw();

diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h
@@ -76,6 +76,15 @@ class AP_L1_Control : public AP_Navigation {
     // reference to the AHRS object
     AP_AHRS &_ahrs;
 
+    enum class NavMode {
+        NONE,
+        WAYPOINT,
+        LOITER,
+        HEADING_HOLD,
+        LEVEL_FLIGHT
+    };
+    NavMode _current_nav_mode = NavMode::NONE;
+
     // lateral acceration in m/s required to fly to the
     // L1 reference point (+ve to right)
     float _latAccDem;