From 5a869174e56045187384267007fc8cd5c128930c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 31 May 2019 21:02:12 +1000 Subject: [PATCH] Copter: use handle_landing_target() for precland allows for jitter correction --- ArduCopter/GCS_Mavlink.cpp | 16 ++++++++++------ ArduCopter/GCS_Mavlink.h | 2 ++ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 4daa0e6b2c..4cf0898390 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -568,6 +568,16 @@ void GCS_MAVLINK_Copter::handle_command_ack(const mavlink_message_t &msg) GCS_MAVLINK::handle_command_ack(msg); } +/* + handle a LANDING_TARGET command. The timestamp has been jitter corrected +*/ +void GCS_MAVLINK_Copter::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) +{ +#if PRECISION_LANDING == ENABLED + copter.precland.handle_msg(packet, timestamp_ms); +#endif +} + MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavlink_command_long_t &packet) { if (is_equal(packet.param6,1.0f)) { @@ -1281,12 +1291,6 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) break; } -#if PRECISION_LANDING == ENABLED - case MAVLINK_MSG_ID_LANDING_TARGET: - copter.precland.handle_msg(msg); - break; -#endif - case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: #if AP_TERRAIN_AVAILABLE && AC_TERRAIN diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index e404b0ddf1..48a47d26b2 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -35,6 +35,8 @@ protected: void handle_mount_message(const mavlink_message_t &msg) override; + void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; void send_nav_controller_output() const override;