From 06eea6ed9fbd34174b17300a55a58b5b1f7a4ad8 Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Wed, 29 Jul 2020 16:01:34 +0100 Subject: [PATCH] Plane: Move navigation functions into flight mode classes. --- ArduPlane/ArduPlane.cpp | 98 ++---------------------------------- ArduPlane/mode.cpp | 5 ++ ArduPlane/mode.h | 13 +++++ ArduPlane/mode_auto.cpp | 7 +++ ArduPlane/mode_avoidADSB.cpp | 5 ++ ArduPlane/mode_cruise.cpp | 32 ++++++++++++ ArduPlane/mode_guided.cpp | 5 ++ ArduPlane/mode_loiter.cpp | 7 +++ ArduPlane/mode_rtl.cpp | 57 +++++++++++++++++++++ ArduPlane/mode_takeoff.cpp | 5 ++ ArduPlane/navigation.cpp | 33 ------------ ArduPlane/quadplane.h | 1 + 12 files changed, 141 insertions(+), 127 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index fcddaf76f6..9b23adb1c8 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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 - // ------------------------------------------------------------------------ - - 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; - } + control_mode->navigate(); } /* diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index e660c7f5c7..9e4b863f7f 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -85,3 +85,8 @@ bool Mode::enter() return enter_result; } +void Mode::navigate() +{ + _navigate(); +} + diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 5cda0ca6ba..a182b4404f 100644 --- a/ArduPlane/mode.h +++ b/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; }; diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 0cb6b26b9a..6005ebfc3a 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -88,3 +88,10 @@ void ModeAuto::update() } } +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 5336a323e6..054410a297 100644 --- a/ArduPlane/mode_avoidADSB.cpp +++ b/ArduPlane/mode_avoidADSB.cpp @@ -11,3 +11,8 @@ void ModeAvoidADSB::update() plane.mode_guided.update(); } +void ModeAvoidADSB::_navigate() +{ + plane.mode_loiter.navigate(); +} + diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 1ea3cdfa70..9b48b4d415 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/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); + } +} + diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 78cc2508c8..4d00218832 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -28,3 +28,8 @@ void ModeGuided::update() } } +void ModeGuided::_navigate() +{ + plane.mode_loiter.navigate(); +} + diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 3501d7553c..30b994e2ac 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/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); +} + diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index ebe218bfa0..7b9aeac347 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/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); +} + diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 3598b59156..e994678a94 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -131,3 +131,8 @@ void ModeTakeoff::update() } } +void ModeTakeoff::_navigate() +{ + plane.mode_loiter.navigate(); +} + diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 2dafc91c84..cfa1d6df93 100644 --- a/ArduPlane/navigation.cpp +++ b/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 diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index c2fd8df125..c1545f96b1 100644 --- a/ArduPlane/quadplane.h +++ b/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;