Plane: Move navigation functions into flight mode classes.

ArduPlane/ArduPlane.cpp

@@ -434,102 +434,12 @@ void Plane::update_control_mode(void)
     effective_mode->update();
 }
 
+/* 
+  call the navigate method of the active flight mode
+*/
 void Plane::update_navigation()
 {
-    // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
+    control_mode->navigate();
-    // ------------------------------------------------------------------------
-
-    uint16_t radius = 0;
-    uint16_t qrtl_radius = abs(g.rtl_radius);
-    if (qrtl_radius == 0) {
-        qrtl_radius = abs(aparm.loiter_radius);
-    }
-    
-    switch (control_mode->mode_number()) {
-    case Mode::Number::AUTO:
-        if (ahrs.home_is_set()) {
-            mission.update();
-        }
-        break;
-            
-    case Mode::Number::RTL:
-        if (quadplane.available() && quadplane.rtl_mode == 1 &&
-            (nav_controller->reached_loiter_target() ||
-             current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc) ||
-             auto_state.wp_distance < MAX(qrtl_radius, quadplane.stopping_distance())) &&
-            AP_HAL::millis() - last_mode_change_ms > 1000) {
-            /*
-              for a quadplane in RTL mode we switch to QRTL when we
-              are within the maximum of the stopping distance and the
-              RTL_RADIUS
-             */
-            set_mode(mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
-            break;
-        } else if (g.rtl_autoland == 1 &&
-            !auto_state.checked_for_autoland &&
-            reached_loiter_target() && 
-            labs(altitude_error_cm) < 1000) {
-            // we've reached the RTL point, see if we have a landing sequence
-            if (mission.jump_to_landing_sequence()) {
-                // switch from RTL -> AUTO
-                mission.set_force_resume(true);
-                set_mode(mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
-            }
-
-            // prevent running the expensive jump_to_landing_sequence
-            // on every loop
-            auto_state.checked_for_autoland = true;
-        }
-        else if (g.rtl_autoland == 2 &&
-            !auto_state.checked_for_autoland) {
-            // Go directly to the landing sequence
-            if (mission.jump_to_landing_sequence()) {
-                // switch from RTL -> AUTO
-                mission.set_force_resume(true);
-                set_mode(mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
-            }
-
-            // prevent running the expensive jump_to_landing_sequence
-            // on every loop
-            auto_state.checked_for_autoland = true;
-        }
-        radius = abs(g.rtl_radius);
-        if (radius > 0) {
-            loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
-        }
-        // fall through to LOITER
-        FALLTHROUGH;
-
-    case Mode::Number::LOITER:
-    case Mode::Number::AVOID_ADSB:
-    case Mode::Number::GUIDED:
-    case Mode::Number::TAKEOFF:
-        update_loiter(radius);
-        break;
-
-    case Mode::Number::CRUISE:
-        update_cruise();
-        break;
-
-    case Mode::Number::MANUAL:
-    case Mode::Number::STABILIZE:
-    case Mode::Number::TRAINING:
-    case Mode::Number::INITIALISING:
-    case Mode::Number::ACRO:
-    case Mode::Number::FLY_BY_WIRE_A:
-    case Mode::Number::AUTOTUNE:
-    case Mode::Number::FLY_BY_WIRE_B:
-    case Mode::Number::CIRCLE:
-    case Mode::Number::QSTABILIZE:
-    case Mode::Number::QHOVER:
-    case Mode::Number::QLOITER:
-    case Mode::Number::QLAND:
-    case Mode::Number::QRTL:
-    case Mode::Number::QAUTOTUNE:
-    case Mode::Number::QACRO:
-        // nothing to do
-        break;
-    }
 }
 
 /*

ArduPlane/mode.cpp

@@ -85,3 +85,8 @@ bool Mode::enter()
     return enter_result;
 }
 
+void Mode::navigate()
+{
+    _navigate();
+}
+

ArduPlane/mode.h

@@ -74,6 +74,9 @@ public:
     // true if mode can have terrain following disabled by switch
     virtual bool allows_terrain_disable() const { return false; }
 
+    // navigation
+    void navigate();
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -81,6 +84,9 @@ protected:
 
     // subclasses override this to perform any required cleanup when exiting the mode
     virtual void _exit() { return; }
+
+    // subclasses override this if they require navigation.
+    virtual void _navigate() { return; }
 };
 
 
@@ -115,6 +121,7 @@ protected:
 
     bool _enter() override;
     void _exit() override;
+    void _navigate() override;
 };
 
 
@@ -149,6 +156,7 @@ public:
 protected:
 
     bool _enter() override;
+    void _navigate() override;
 };
 
 class ModeCircle: public Mode
@@ -184,6 +192,7 @@ public:
 protected:
 
     bool _enter() override;
+    void _navigate() override;
 };
 
 class ModeManual : public Mode
@@ -218,6 +227,7 @@ public:
 protected:
 
     bool _enter() override;
+    void _navigate() override;
 };
 
 class ModeStabilize : public Mode
@@ -319,6 +329,7 @@ public:
 protected:
 
     bool _enter() override;
+    void _navigate() override;
 };
 
 class ModeAvoidADSB : public Mode
@@ -335,6 +346,7 @@ public:
 protected:
 
     bool _enter() override;
+    void _navigate() override;
 };
 
 class ModeQStabilize : public Mode
@@ -498,4 +510,5 @@ protected:
     Location start_loc;
 
     bool _enter() override;
+    void _navigate() override;
 };

ArduPlane/mode_auto.cpp

@@ -88,3 +88,10 @@ void ModeAuto::update()
     }
 }
 
+void ModeAuto::_navigate()
+{
+    if (AP::ahrs().home_is_set()) {
+        plane.mission.update();
+    }
+}
+

ArduPlane/mode_avoidADSB.cpp

@@ -11,3 +11,8 @@ void ModeAvoidADSB::update()
     plane.mode_guided.update();
 }
 
+void ModeAvoidADSB::_navigate()
+{
+    plane.mode_loiter.navigate();
+}
+

ArduPlane/mode_cruise.cpp

@@ -40,3 +40,35 @@ void ModeCruise::update()
     plane.update_fbwb_speed_height();
 }
 
+/*
+  handle CRUISE mode, locking heading to GPS course when we have
+  sufficient ground speed, and no aileron or rudder input
+ */
+void ModeCruise::_navigate()
+{
+    if (!plane.cruise_state.locked_heading &&
+        plane.channel_roll->get_control_in() == 0 &&
+        plane.rudder_input() == 0 &&
+        plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
+        plane.gps.ground_speed() >= 3 &&
+        plane.cruise_state.lock_timer_ms == 0) {
+        // user wants to lock the heading - start the timer
+        plane.cruise_state.lock_timer_ms = millis();
+    }
+    if (plane.cruise_state.lock_timer_ms != 0 &&
+        (millis() - plane.cruise_state.lock_timer_ms) > 500) {
+        // lock the heading after 0.5 seconds of zero heading input
+        // from user
+        plane.cruise_state.locked_heading = true;
+        plane.cruise_state.lock_timer_ms = 0;
+        plane.cruise_state.locked_heading_cd = plane.gps.ground_course_cd();
+        plane.prev_WP_loc = plane.current_loc;
+    }
+    if (plane.cruise_state.locked_heading) {
+        plane.next_WP_loc = plane.prev_WP_loc;
+        // always look 1km ahead
+        plane.next_WP_loc.offset_bearing(plane.cruise_state.locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
+        plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
+    }
+}
+

ArduPlane/mode_guided.cpp

@@ -28,3 +28,8 @@ void ModeGuided::update()
     }
 }
 
+void ModeGuided::_navigate()
+{
+    plane.mode_loiter.navigate();
+}
+

ArduPlane/mode_loiter.cpp

@@ -80,3 +80,10 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd)
     }
     return false;
 }
+
+void ModeLoiter::_navigate()
+{
+    // Zero indicates to use WP_LOITER_RAD
+    plane.update_loiter(0);
+}
+

ArduPlane/mode_rtl.cpp

@@ -34,3 +34,60 @@ void ModeRTL::update()
     }
 }
 
+void ModeRTL::_navigate()
+{
+    uint16_t qrtl_radius = abs(plane.g.rtl_radius);
+    if (qrtl_radius == 0) {
+        qrtl_radius = abs(plane.aparm.loiter_radius);
+    }
+
+    if (plane.quadplane.available() && plane.quadplane.rtl_mode == 1 &&
+        (plane.nav_controller->reached_loiter_target() ||
+         plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) ||
+         plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance())) &&
+        AP_HAL::millis() - plane.last_mode_change_ms > 1000) {
+        /*
+          for a quadplane in RTL mode we switch to QRTL when we
+          are within the maximum of the stopping distance and the
+          RTL_RADIUS
+         */
+        plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
+        return;
+    }
+
+    if (plane.g.rtl_autoland == 1 &&
+        !plane.auto_state.checked_for_autoland &&
+        plane.reached_loiter_target() && 
+        labs(plane.altitude_error_cm) < 1000) {
+        // we've reached the RTL point, see if we have a landing sequence
+        if (plane.mission.jump_to_landing_sequence()) {
+            // switch from RTL -> AUTO
+            plane.mission.set_force_resume(true);
+            plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
+        }
+
+        // prevent running the expensive jump_to_landing_sequence
+        // on every loop
+        plane.auto_state.checked_for_autoland = true;
+    }
+    else if (plane.g.rtl_autoland == 2 &&
+        !plane.auto_state.checked_for_autoland) {
+        // Go directly to the landing sequence
+        if (plane.mission.jump_to_landing_sequence()) {
+            // switch from RTL -> AUTO
+            plane.mission.set_force_resume(true);
+            plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND);
+        }
+
+        // prevent running the expensive jump_to_landing_sequence
+        // on every loop
+        plane.auto_state.checked_for_autoland = true;
+    }
+    uint16_t radius = abs(plane.g.rtl_radius);
+    if (radius > 0) {
+        plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1;
+    }
+
+    plane.update_loiter(radius);
+}
+

ArduPlane/mode_takeoff.cpp

@@ -131,3 +131,8 @@ void ModeTakeoff::update()
     }
 }
 
+void ModeTakeoff::_navigate()
+{
+    plane.mode_loiter.navigate();
+}
+

ArduPlane/navigation.cpp

@@ -278,39 +278,6 @@ void Plane::update_loiter(uint16_t radius)
     }
 }
 
-/*
-  handle CRUISE mode, locking heading to GPS course when we have
-  sufficient ground speed, and no aileron or rudder input
- */
-void Plane::update_cruise()
-{
-    if (!cruise_state.locked_heading &&
-        channel_roll->get_control_in() == 0 &&
-        rudder_input() == 0 &&
-        gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
-        gps.ground_speed() >= 3 &&
-        cruise_state.lock_timer_ms == 0) {
-        // user wants to lock the heading - start the timer
-        cruise_state.lock_timer_ms = millis();
-    }
-    if (cruise_state.lock_timer_ms != 0 &&
-        (millis() - cruise_state.lock_timer_ms) > 500) {
-        // lock the heading after 0.5 seconds of zero heading input
-        // from user
-        cruise_state.locked_heading = true;
-        cruise_state.lock_timer_ms = 0;
-        cruise_state.locked_heading_cd = gps.ground_course_cd();
-        prev_WP_loc = current_loc;
-    }
-    if (cruise_state.locked_heading) {
-        next_WP_loc = prev_WP_loc;
-        // always look 1km ahead
-        next_WP_loc.offset_bearing(cruise_state.locked_heading_cd*0.01f, prev_WP_loc.get_distance(current_loc) + 1000);
-        nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
-    }
-}
-
-
 /*
   handle speed and height control in FBWB or CRUISE mode. 
   In this mode the elevator is used to change target altitude. The

ArduPlane/quadplane.h

@@ -30,6 +30,7 @@ public:
 
     friend class Mode;
     friend class ModeAuto;
+    friend class ModeRTL;
     friend class ModeAvoidADSB;
     friend class ModeGuided;
     friend class ModeQHover;