diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 56cd867f692ef..60a22e44fe412 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -857,21 +857,20 @@ bool Plane::get_target_location(Location& target_loc) */ bool Plane::update_target_location(const Location &old_loc, const Location &new_loc) { - if (!old_loc.same_loc_as(next_WP_loc)) { + /* + by checking the caller has provided the correct old target + location we prevent a race condition where the user changes mode + or commands a different target in the controlling lua script + */ + if (!old_loc.same_loc_as(next_WP_loc) || + old_loc.get_alt_frame() != new_loc.get_alt_frame()) { return false; } next_WP_loc = new_loc; - next_WP_loc.change_alt_frame(old_loc.get_alt_frame()); #if HAL_QUADPLANE_ENABLED if (control_mode == &mode_qland) { - /* - support precision landing controlled by lua in QLAND mode - */ - Vector2f rel_origin; - if (new_loc.get_vector_xy_from_origin_NE(rel_origin)) { - quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y); - } + mode_qland.last_target_loc_set_ms = AP_HAL::millis(); } #endif @@ -895,7 +894,8 @@ bool Plane::set_velocity_match(const Vector2f &velocity) bool Plane::set_land_descent_rate(float descent_rate) { #if HAL_QUADPLANE_ENABLED - if (quadplane.in_vtol_land_descent()) { + if (quadplane.in_vtol_land_descent() || + control_mode == &mode_qland) { quadplane.poscontrol.override_descent_rate = descent_rate; quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis(); return true; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index dec67d5a2bedd..dc97584962ad1 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -748,6 +748,16 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c } +/* + handle a LANDING_TARGET command. The timestamp has been jitter corrected +*/ +void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) +{ +#if AC_PRECLAND_ENABLED + plane.g2.precland.handle_msg(packet, timestamp_ms); +#endif +} + MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { plane.in_calibration = true; diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 5920a354028bf..b3b59cb2d8790 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -50,6 +50,7 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK void send_pid_tuning() override; void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; + void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; private: diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 597399cc21676..076d51f87026f 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -669,6 +669,7 @@ friend class ModeQLand; class ModeQLand : public Mode { public: + friend class Plane; Number mode_number() const override { return Number::QLAND; } const char *name() const override { return "QLAND"; } @@ -686,6 +687,7 @@ class ModeQLand : public Mode 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 e646c80746817..681f7cb26cfee 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -19,6 +19,10 @@ bool ModeQLand::_enter() #if AP_FENCE_ENABLED plane.fence.auto_disable_fence_for_landing(); #endif + + // clear precland timestamp + last_target_loc_set_ms = 0; + return true; } @@ -29,6 +33,33 @@ 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 + */ plane.mode_qloiter.run(); }