From 980d328b537d876b2edc4f204d1522fe17939755 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Mar 2024 16:05:41 +1100 Subject: [PATCH] Plane: make precsion QLOITER possible from lua --- ArduPlane/ArduPlane.cpp | 7 +++---- ArduPlane/RC_Channel.cpp | 5 +++++ ArduPlane/mode.h | 7 +++---- ArduPlane/mode_qland.cpp | 27 --------------------------- ArduPlane/mode_qloiter.cpp | 34 ++++++++++++++++++++++++++++++++++ 5 files changed, 45 insertions(+), 35 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 60a22e44fe..2e805513ab 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -869,8 +869,8 @@ bool Plane::update_target_location(const Location &old_loc, const Location &new_ next_WP_loc = new_loc; #if HAL_QUADPLANE_ENABLED - if (control_mode == &mode_qland) { - mode_qland.last_target_loc_set_ms = AP_HAL::millis(); + if (control_mode == &mode_qland || control_mode == &mode_qloiter) { + mode_qloiter.last_target_loc_set_ms = AP_HAL::millis(); } #endif @@ -966,8 +966,7 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const void Plane::precland_update(void) { // alt will be unused if we pass false through as the second parameter: - return g2.precland.update(rangefinder_state.height_estimate*100, - rangefinder_state.in_range && rangefinder_state.in_use); + return g2.precland.update(rangefinder_state.height_estimate*100, rangefinder_state.in_range); } #endif diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index d61413c3d0..1fc5b0ad22 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -177,6 +177,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, case AUX_FUNC::EMERGENCY_LANDING_EN: case AUX_FUNC::FW_AUTOTUNE: case AUX_FUNC::VFWD_THR_OVERRIDE: + case AUX_FUNC::PRECISION_LOITER: break; case AUX_FUNC::SOARING: @@ -443,6 +444,10 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch } break; + case AUX_FUNC::PRECISION_LOITER: + // handled by lua scripting, just ignore here + break; + default: return RC_Channel::do_aux_function(ch_option, ch_flag); } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 076d51f870..a217c2bf7d 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -647,6 +647,8 @@ class ModeQLoiter : public Mode { friend class QuadPlane; friend class ModeQLand; +friend class Plane; + public: Number mode_number() const override { return Number::QLOITER; } @@ -664,13 +666,12 @@ public: protected: bool _enter() override; + uint32_t last_target_loc_set_ms; }; class ModeQLand : public Mode { public: - friend class Plane; - Number mode_number() const override { return Number::QLAND; } const char *name() const override { return "QLAND"; } const char *name4() const override { return "QLND"; } @@ -686,8 +687,6 @@ protected: bool _enter() override; bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; } - - uint32_t last_target_loc_set_ms; }; class ModeQRTL : public Mode diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index 681f7cb26c..4205960f87 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -20,9 +20,6 @@ bool ModeQLand::_enter() plane.fence.auto_disable_fence_for_landing(); #endif - // clear precland timestamp - last_target_loc_set_ms = 0; - return true; } @@ -33,30 +30,6 @@ void ModeQLand::update() void ModeQLand::run() { - /* - see if precision landing is active with an override of the - target location - */ - const uint32_t last_pos_set_ms = last_target_loc_set_ms; - const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms; - const uint32_t now_ms = AP_HAL::millis(); - - if (last_pos_set_ms != 0 && now_ms - last_pos_set_ms < 500) { - // we have an active landing target override - Vector2f rel_origin; - if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) { - quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y); - } - } - - // allow for velocity override as well - if (last_vel_set_ms != 0 && now_ms - last_vel_set_ms < 500) { - // we have an active landing velocity override - Vector2f target_accel; - Vector2f target_speed_xy_cms{quadplane.poscontrol.velocity_match.x*100, quadplane.poscontrol.velocity_match.y*100}; - quadplane.pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel); - } - /* use QLOITER to do the main control */ diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index fdadd0e255..90a4d9f227 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -17,6 +17,10 @@ bool ModeQLoiter::_enter() // prevent re-init of target position quadplane.last_loiter_ms = AP_HAL::millis(); + + // clear precland timestamp + last_target_loc_set_ms = 0; + return true; } @@ -29,6 +33,36 @@ void ModeQLoiter::update() void ModeQLoiter::run() { const uint32_t now = AP_HAL::millis(); + +#if AC_PRECLAND_ENABLED + const uint32_t precland_timeout_ms = 250; + /* + see if precision landing or precision loiter is active with + an override of the target location. + + */ + const uint32_t last_pos_set_ms = last_target_loc_set_ms; + const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms; + + if (last_pos_set_ms != 0 && now - last_pos_set_ms < precland_timeout_ms) { + // we have an active landing target override + Vector2f rel_origin; + if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) { + quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y); + last_target_loc_set_ms = 0; + } + } + + // allow for velocity override as well + if (last_vel_set_ms != 0 && now - last_vel_set_ms < precland_timeout_ms) { + // we have an active landing velocity override + Vector2f target_accel; + Vector2f target_speed_xy_cms{quadplane.poscontrol.velocity_match.x*100, quadplane.poscontrol.velocity_match.y*100}; + quadplane.pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel); + quadplane.poscontrol.last_velocity_match_ms = 0; + } +#endif // AC_PRECLAND_ENABLED + if (quadplane.tailsitter.in_vtol_transition(now)) { // Tailsitters in FW pull up phase of VTOL transition run FW controllers Mode::run();