diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 9e4b863f7f..e660c7f5c7 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -85,8 +85,3 @@ bool Mode::enter() return enter_result; } -void Mode::navigate() -{ - _navigate(); -} - diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index a182b4404f..230543e844 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -74,8 +74,8 @@ public: // true if mode can have terrain following disabled by switch virtual bool allows_terrain_disable() const { return false; } - // navigation - void navigate(); + // subclasses override this if they require navigation. + virtual void navigate() { return; } protected: @@ -84,9 +84,6 @@ 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; } }; @@ -117,11 +114,12 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + void navigate() override; + protected: bool _enter() override; void _exit() override; - void _navigate() override; }; @@ -153,10 +151,11 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + void navigate() override; + protected: bool _enter() override; - void _navigate() override; }; class ModeCircle: public Mode @@ -186,13 +185,14 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + void navigate() override; + bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc); bool isHeadingLinedUp_cd(const int32_t bearing_cd); protected: bool _enter() override; - void _navigate() override; }; class ModeManual : public Mode @@ -224,10 +224,11 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + void navigate() override; + protected: bool _enter() override; - void _navigate() override; }; class ModeStabilize : public Mode @@ -326,10 +327,11 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + void navigate() override; + protected: bool _enter() override; - void _navigate() override; }; class ModeAvoidADSB : public Mode @@ -343,10 +345,10 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + void navigate() override; protected: bool _enter() override; - void _navigate() override; }; class ModeQStabilize : public Mode @@ -497,6 +499,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + void navigate() override; + // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -510,5 +514,4 @@ protected: Location start_loc; bool _enter() override; - void _navigate() override; }; diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 6005ebfc3a..19994725e8 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -88,7 +88,7 @@ void ModeAuto::update() } } -void ModeAuto::_navigate() +void ModeAuto::navigate() { if (AP::ahrs().home_is_set()) { plane.mission.update(); diff --git a/ArduPlane/mode_avoidADSB.cpp b/ArduPlane/mode_avoidADSB.cpp index 6c87688724..897084bf60 100644 --- a/ArduPlane/mode_avoidADSB.cpp +++ b/ArduPlane/mode_avoidADSB.cpp @@ -11,7 +11,7 @@ void ModeAvoidADSB::update() plane.mode_guided.update(); } -void ModeAvoidADSB::_navigate() +void ModeAvoidADSB::navigate() { // Zero indicates to use WP_LOITER_RAD plane.update_loiter(0); diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 9b48b4d415..7aa86d9adf 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -44,7 +44,7 @@ void ModeCruise::update() handle CRUISE mode, locking heading to GPS course when we have sufficient ground speed, and no aileron or rudder input */ -void ModeCruise::_navigate() +void ModeCruise::navigate() { if (!plane.cruise_state.locked_heading && plane.channel_roll->get_control_in() == 0 && diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 9ae854760c..d1bc47cc27 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -28,7 +28,7 @@ void ModeGuided::update() } } -void ModeGuided::_navigate() +void ModeGuided::navigate() { // Zero indicates to use WP_LOITER_RAD plane.update_loiter(0); diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 30b994e2ac..645bb647d2 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -81,7 +81,7 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) return false; } -void ModeLoiter::_navigate() +void ModeLoiter::navigate() { // Zero indicates to use WP_LOITER_RAD plane.update_loiter(0); diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 7b9aeac347..bd581a5793 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -34,7 +34,7 @@ void ModeRTL::update() } } -void ModeRTL::_navigate() +void ModeRTL::navigate() { uint16_t qrtl_radius = abs(plane.g.rtl_radius); if (qrtl_radius == 0) { diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 1683508966..7878a6ce70 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -131,7 +131,7 @@ void ModeTakeoff::update() } } -void ModeTakeoff::_navigate() +void ModeTakeoff::navigate() { // Zero indicates to use WP_LOITER_RAD plane.update_loiter(0);