From 4eeb1f18d3ae17bb709aa2271dc2e456c5956272 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 21 Jun 2023 12:30:05 -0700 Subject: [PATCH] Plane: fixup is_landing and is_taking_off members --- ArduPlane/ArduPlane.cpp | 22 ++++++++++++++++++++++ ArduPlane/Plane.cpp | 22 ---------------------- ArduPlane/Plane.h | 1 - ArduPlane/mode.cpp | 5 +++++ ArduPlane/mode.h | 6 +----- ArduPlane/mode_auto.cpp | 19 +------------------ 6 files changed, 29 insertions(+), 46 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ed14053d0b..d6bdeb797f 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -851,6 +851,28 @@ bool Plane::set_land_descent_rate(float descent_rate) return false; } +// returns true if vehicle is landing. +bool Plane::is_landing() const +{ +#if HAL_QUADPLANE_ENABLED + if (plane.quadplane.in_vtol_land_descent()) { + return true; + } +#endif + return control_mode->is_landing(); +} + +// returns true if vehicle is taking off. +bool Plane::is_taking_off() const +{ +#if HAL_QUADPLANE_ENABLED + if (plane.quadplane.in_vtol_takeoff()) { + return true; + } +#endif + return control_mode->is_taking_off(); +} + #endif // AP_SCRIPTING_ENABLED // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index fa3a4eb6d4..dff3166b02 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -32,25 +32,3 @@ Plane::Plane(void) Plane plane; AP_Vehicle& vehicle = plane; - -#if AP_SCRIPTING_ENABLED - -// returns true if vehicle is landing. Only used by Lua scripts -bool Plane::is_landing() const -{ -#if HAL_QUADPLANE_ENABLED - if (plane.quadplane.in_vtol_land_descent()) { - return true; - } -#endif - - return control_mode->is_landing(); -} - -// returns true if vehicle is taking off. Only used by Lua scripts -bool Plane::is_taking_off() const -{ - return control_mode->is_taking_off() || (started_flying_ms > 0 && millis() - started_flying_ms < 30000); -} - -#endif // AP_SCRIPTING_ENABLED diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d26500571b..4a5dd56030 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -523,7 +523,6 @@ private: float rudder_offset_pct; bool run_yaw_rate_controller; } nav_scripting; - #endif struct { diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index f8291b1696..a1fdb5ac2d 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -225,3 +225,8 @@ void Mode::reset_controllers() plane.steer_state.locked_course = false; plane.steer_state.locked_course_err = 0; } + +bool Mode::is_taking_off() const +{ + return (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF); +} diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 1e2701dd7d..ec94ed4457 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -132,7 +132,7 @@ public: virtual bool is_landing() const { return false; } // true if is taking - virtual bool is_taking_off() const { return false; } + virtual bool is_taking_off() const; protected: @@ -218,8 +218,6 @@ public: bool is_landing() const override; - bool is_taking_off() const override; - protected: bool _enter() override; @@ -756,8 +754,6 @@ public: bool does_auto_throttle() const override { return true; } - bool is_taking_off() const override { return true; } - // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index ea6af90f2c..6ebf3546ef 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -162,22 +162,5 @@ bool ModeAuto::_pre_arm_checks(size_t buflen, char *buffer) const bool ModeAuto::is_landing() const { - switch(plane.mission.get_current_nav_cmd().id) { - case MAV_CMD_NAV_LAND: - case MAV_CMD_NAV_VTOL_LAND: - case MAV_CMD_DO_VTOL_TRANSITION: - return true; - } - return plane.mission.get_in_landing_sequence_flag(); -} - -bool ModeAuto::is_taking_off() const -{ - switch(plane.mission.get_current_nav_cmd().id) { - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_NAV_VTOL_TAKEOFF: - case MAV_CMD_DO_VTOL_TRANSITION: - return true; - } - return false; + return (plane.flight_stage == AP_FixedWing::FlightStage::LAND); }